CN110076783B - Planar under-actuated mechanical arm position control method based on fuzzy control and Lyapunov function - Google Patents

Planar under-actuated mechanical arm position control method based on fuzzy control and Lyapunov function Download PDF

Info

Publication number
CN110076783B
CN110076783B CN201910517174.8A CN201910517174A CN110076783B CN 110076783 B CN110076783 B CN 110076783B CN 201910517174 A CN201910517174 A CN 201910517174A CN 110076783 B CN110076783 B CN 110076783B
Authority
CN
China
Prior art keywords
joint
angle
mechanical arm
target
control
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.)
Expired - Fee Related
Application number
CN201910517174.8A
Other languages
Chinese (zh)
Other versions
CN110076783A (en
Inventor
高欣
翟林
任泽宇
刘惠禾
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing University of Posts and Telecommunications
Original Assignee
Beijing University of Posts and Telecommunications
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing University of Posts and Telecommunications filed Critical Beijing University of Posts and Telecommunications
Priority to CN201910517174.8A priority Critical patent/CN110076783B/en
Publication of CN110076783A publication Critical patent/CN110076783A/en
Application granted granted Critical
Publication of CN110076783B publication Critical patent/CN110076783B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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 planar under-actuated mechanical arm position control method based on fuzzy control and a Lyapunov function. The method comprises the following steps: establishing a planar three-degree-of-freedom passive-active (PAA type) under-actuated mechanical arm dynamic model, and analyzing the integral constraint relation between the angle and the angular speed of each joint; analyzing the reachable space of the joint, and calculating the target angle of each driving joint by using a particle swarm algorithm; designing a segmentation position control law based on fuzzy control and a Lyapunov function, designing a fuzzy controller to enable a second joint to reach a target angle in a first stage, and designing the Lyapunov function to enable a third joint to keep an initial state; and designing a fuzzy controller at the second stage to enable the third joint to reach a target angle, and designing a Lyapunov function to enable the second joint to keep the angle unchanged, so as to realize position control. The technical scheme provided by the embodiment of the invention can ensure the stability of position control, and the overshoot and the steady-state time are obviously reduced.

Description

Planar under-actuated mechanical arm position control method based on fuzzy control and Lyapunov function
[ technical field ] A method for producing a semiconductor device
The invention belongs to the technical field of automatic control, relates to a model order reduction method, a fuzzy control method and a Lyapunov direct control method, and particularly relates to a planar under-actuated mechanical arm sectional position control method based on fuzzy control and a Lyapunov function.
[ background of the invention ]
With the development of aerospace technology and robot control technology, especially for the practical situation that a space manipulator is difficult to maintain and an underwater robot has a compact structural design, research on under-actuated mechanical systems is receiving wide attention all over the world. Because the number of external control inputs of the under-actuated mechanical system is less than the number of degrees of freedom, mature control methods in the fully-actuated mechanical arm, such as a full-state feedback linearization method, a PID control method, a passive-based adaptive control method and the like, cannot be directly popularized to the under-actuated mechanical arm. Therefore, the problem of control of under-actuated robotic arms is a recognized challenge in the field.
The under-actuated mechanical arm can be divided into a horizontal plane motion and a vertical plane motion according to the structure. The underactuated mechanical arm moving in a vertical plane only has two balance positions, namely a highest balance position and a lowest balance position. However, the horizontal underactuated robot arm is not affected by gravity and has an infinite number of equilibrium positions. And the horizontal under-actuated mechanical arm with the first joint being a passive joint is an incomplete system, which makes the position control thereof more difficult. And reducing the order of the three-degree-of-freedom underactuated mechanical arm into two-degree-of-freedom mechanical arms by utilizing the idea of model order reduction. In the existing control method, a control law is derived only by constructing a Lyapunov function according to a state equation of a system. This method can design a stable control law, but it is difficult to find an optimal lyapunov function, so the rapidity of control is not good. The position control of a plane PAA type mechanical arm is realized based on a single sliding mode control method, the mechanical arm is divided into a plurality of subsystems, a sliding mode surface and a switching function are respectively designed for each subsystem, and a total sliding mode surface is constructed, so that the whole system is stable. However, this method has a problem of chattering and is poor in steady-state performance. The dynamic characteristics of the control process are also in need of optimization.
[ summary of the invention ]
In view of the above, in view of the problem that the existing control method is difficult to consider both rapidity and accuracy, the invention provides a method for controlling the segmented position of a planar under-actuated mechanical arm based on fuzzy control and a lyapunov function, which comprises the following steps:
establishing a planar three-degree-of-freedom passive-active (PAA type) under-actuated mechanical arm dynamic model, and analyzing the integral constraint relation between the angle and the angular speed of each joint;
analyzing the reachable space of the joint, and calculating the target angle of each driving joint by using a particle swarm algorithm;
designing a segmentation position control law based on fuzzy control and a Lyapunov function, designing a fuzzy controller to enable a second joint to reach a target angle in a first stage, and designing the Lyapunov function to enable a third joint to keep an initial state; and designing a fuzzy controller at the second stage to enable the third joint to reach a target angle, and designing a Lyapunov function to enable the second joint to keep the angle unchanged, so as to realize position control.
In the method, a planar three-degree-of-freedom passive-active (PAA type) under-actuated mechanical arm dynamic model is established, and an integral constraint relation between angles and angular velocities of each joint is analyzed, wherein the method comprises the following steps:
the plane three-degree-of-freedom passive-active (PAA type) under-actuated mechanical arm is not influenced by gravity, so potential energy is not considered, a passive joint can rotate freely, braking torque and friction force are not generated, a research object is simplified into a rigid body structure, and a dynamic model is established according to a Lagrange equation as follows:
Figure BDA0002095417580000021
wherein M isij∈R3×3(i, j-1, 2,3) specifically,
Figure BDA0002095417580000031
ak(k ═ 1, 2.., 6) is a structural parameter of the system, q is a structural parameter of the systemi(i is 1,2,3) is the ith joint angle of the mechanical arm,
Figure BDA0002095417580000032
the ith joint angular acceleration of the mechanical arm;
Figure BDA0002095417580000033
miis the mass of the ith rod (i ═ 1,2,3), LiLength of ith rod (i ═ 1,2,3), liLength from ith rod centroid to previous joint (i ═ 1,2,3), JiMoment of inertia (i ═ 1,2,3) for the ith rod; hi∈R3×1(i is 1,2,3) is the terms of the coriolis force and the centrifugal force,
Figure BDA0002095417580000034
τ＝(0,τ2,τ3)Tis a joint torque vector, wherein the output torque of the passive joint is 0,
Figure BDA0002095417580000035
the ith joint angular velocity of the mechanical arm;
the plane PAA type under-actuated mechanical arm is a first-order incomplete system, and the plane PA type mechanical arm is an complete system; based on the idea of model order reduction, the control process is divided into two stages, the initial angle of the third joint is maintained to be unchanged in the first stage, and the second joint is controlled to reach the target angle; in the second stage, the target angle of the second joint is kept unchanged, and the third joint is controlled to reach the target angle; the line where underactuation is located is extracted in equation (1), and equation (4) is substituted to obtain:
Figure BDA0002095417580000036
by using fractional integration, equation (5) integrates time t to obtain:
Figure BDA0002095417580000041
the angle is kept constant in each control stage, and the angular velocity of the joint can be approximately considered
Figure BDA0002095417580000042
Zero, therefore, the integral is determined again across equation (6), simplifying to obtain:
Figure BDA0002095417580000043
wherein q is1 1(0) Representing the initial angle of the passive rod in the first stage, q2(0) And q is3(0) Representing the initial angles, D, of the second and third bars, respectively1,E1,G1,g1For the intermediate term of the reduction process of equation (7),
D1＝(8a3a5-4a2a6-4a1a6-4a4a6)cosq2(0)-a1 2-a2 2-4a6 2cos2q2(0)-a4 2+4a5 2+4a3 2-2a1a2-2a1a4-2a2a4
Figure BDA0002095417580000044
G1＝(a1+a2+a4+2a6cosq2(0)-2a5cosq2(0)-2a3)(cosq2-1)+2a5sinq2(0)sinq2
Figure BDA0002095417580000045
similarly, the second joint angle is maintained in the second stage of control, i.e., the joint angular velocity can be approximated
Figure BDA0002095417580000049
And (4) obtaining the second stage joint angle constraint by determining the integral at the two ends of the equation (6) again.
In the above method, analyzing the reachable space, and calculating the target angle of each driving joint by using a particle swarm algorithm includes:
the working space of the mechanical arm refers to a set of spatial positions that can be reached by the end effector of the mechanical arm, the working domain of the mechanical arm is denoted as a (p), and then the mapping of each joint angle of the mechanical arm and the working reachable space can be expressed as:
A(p)＝{p(θ)|θ∈Q} (8)
wherein Q is the constraint space of joint angle to the constraint of arm angle is regarded as to the joint spacing angle, promptly:
Figure BDA0002095417580000046
in the formula (I), the compound is shown in the specification,
Figure BDA0002095417580000047
and
Figure BDA0002095417580000048
denotes the lower and upper limits of joint movement, thetanIndicating the nth joint angle. In addition, angle constraint exists between the active joint and the passive joint of the PA type mechanical arm, and a joint angle constraint space is added; the essence of calculating the working space of the multi-joint mechanical arm by using the Monte Carlo method is that a certain amount of random quantities meeting the joint change requirements are given to joint variables through uniform distribution, then all the joint variables are combined, the coordinate values of the end point of the end effector of the mechanical arm are calculated by using the forward kinematics recursion of the mechanical arm, and the set formed by the coordinate values is the reachable space of the tail end of the mechanical arm;
the cartesian coordinates of the end position of the planar PAA type under-actuated manipulator can be expressed as:
Figure BDA0002095417580000051
wherein (X, Y) are cartesian coordinates of the end expectation; selecting a difference value between the tail end coordinate of the mechanical arm and an expected position as an evaluation function, wherein the evaluation function can enable the particle swarm to continuously move to an optimal area in a solution space, and further a target angle of the driving rod is obtained;
the particle swarm optimization algorithm comprises the following steps:
1) randomly generating an initial population of m particles, initializing the speed and position of each particle, and giving an inertia weight omega, learning factors c1 and c2, pbest and gbest;
2) calculating the fitness of each particle;
3) updating the flight position and the flight speed of each particle according to the equations (11) and (12);
vi＝vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbesti-xi) (11)
xi＝xi+vi (12)
4) if the fitness of the particles is superior to the original individual extreme value pbest, the current fitness is set as pbest, and the optimal individual extreme value is selected as a group extreme value gbest;
5) and judging whether the algorithm reaches a termination condition. If yes, outputting a result; otherwise, performing a loop part from 1 to M, wherein M is the maximum iteration number;
and calculating the target angles of the two driving joints by using a particle swarm algorithm to lay a foundation for subsequent position control.
In the method, a segmentation position control law based on fuzzy control and a Lyapunov function is designed, a fuzzy controller is designed in a first stage to enable a second joint to reach a target angle, and the Lyapunov function is designed to enable a third joint to keep an initial state; designing a fuzzy controller at the second stage to enable the third joint to reach a target angle, designing a Lyapunov function to enable the second joint to keep the angle unchanged, and realizing position control, wherein the position control comprises the following steps:
first, the kinetic model is rewritten into an affine nonlinear system form as follows:
Figure BDA0002095417580000061
wherein F, G are both non-linear functions with respect to the state variable x,
Figure BDA0002095417580000062
Figure BDA0002095417580000063
[G1 G2 G3]T＝-M-1(q); according to the definition of the Lyapunov function and the control target of the mechanical arm tail end at the target point stability, the function is constructed to ensure that the angle of the driving connecting rod finally tends to the target angle and the angular speed of the driving connecting rod is enabled to be simultaneouslyThe degree eventually converges to zero, so the third link lyapunov function is constructed as follows:
Figure BDA0002095417580000064
wherein, V3(x) For a semi-positive definite function, the above equation is derived and the system state equation (12) is substituted to obtain:
Figure BDA0002095417580000065
wherein a is22For the second row and second column entries in the G matrix, the control law can be found according to the following equation:
τ3＝(-x5+x5(0)-F3-μx5-aa22τ2)/aa22 (16)
where μ is a constant greater than 0, for adjusting the convergence rate; x is the number of5(0) Is the initial setting of the third joint angle. It can be seen that the output torque of the third joint changes with the output torque of the second joint, and the third joint is stabilized at the set initial angle regardless of the change in the angle of the second joint. Designing a fuzzy control law of a second connecting rod, taking the joint angle error and the error change rate as fuzzy system input, taking the moment of a second joint as output, performing defuzzification by adopting a gravity center method, and entering a second stage by a control strategy when the angle of the second joint reaches a target position and the angular speed is 0; similar to the first stage, respectively constructing a Lyapunov function of the second joint, keeping the initial angle of the second joint unchanged, and designing a fuzzy controller to realize the control target of the third joint; when the angles of the driving rods are respectively stabilized at respective target angles, the angle control of the under-actuated connecting rod is jointly realized according to the constraint relation among the angles, and finally the position control of the PAA type mechanical arm is realized;
[ description of the drawings ]
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without inventive labor.
FIG. 1 is a block diagram illustrating an exemplary control method of the present invention;
FIG. 2 illustrates the robot arm reach space;
FIG. 3 shows the relationship of the position of the end of the robot arm to the respective joint angles;
FIG. 4 shows a joint angle membership function;
FIG. 5 shows joint angular velocity membership functions;
FIG. 6 illustrates a position control change process;
FIG. 7 shows a first stage control curve;
fig. 8 shows a second stage control curve.
[ detailed description ] embodiments
For better understanding of the technical solutions of the present invention, the following detailed description of the embodiments of the present invention is provided with reference to the accompanying drawings.
It should be understood that the described embodiments are only some embodiments of the invention, and not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
A planar under-actuated mechanical arm segmented position control method based on fuzzy control and a Lyapunov function specifically comprises the following steps:
1. establishing a planar three-degree-of-freedom passive-active (PAA type) under-actuated mechanical arm dynamic model, and analyzing the integral constraint relation between the angle and the angular speed of each joint; 2. analyzing the reachable space of the joint, and calculating the target angle of each driving joint by using a particle swarm algorithm; 3. designing a segmentation position control law based on fuzzy control and a Lyapunov function, designing a fuzzy controller to enable a second joint to reach a target angle in a first stage, and designing the Lyapunov function to enable a third joint to keep an initial state; and designing a fuzzy controller at the second stage to enable the third joint to reach a target angle, and designing a Lyapunov function to enable the second joint to keep the angle unchanged, so as to realize position control.
The embodiment of the invention provides a sectional position control method for a planar under-actuated mechanical arm based on a fuzzy function and a lyapunov function, and please refer to fig. 1, which is a control process block diagram of the sectional position control method for the planar under-actuated mechanical arm based on the fuzzy function and the lyapunov function, and the method comprises the following steps:
step 101, establishing a planar three-degree-of-freedom passive-active (PAA type) under-actuated mechanical arm dynamic model, decomposing a control process into two control stages according to model reduction, and analyzing an integral constraint relation between angles and angular velocities of joints;
specifically, a planar three-degree-of-freedom passive-active (PAA-type) under-actuated mechanical arm dynamics model is established as follows:
Figure BDA0002095417580000081
wherein M isij∈R3×3(i, j-1, 2,3) specifically,
Figure BDA0002095417580000082
ak(k ═ 1, 2.., 6) is a structural parameter of the system, q is a structural parameter of the systemi(i is 1,2,3) is the ith joint angle of the mechanical arm,
Figure BDA0002095417580000083
the ith joint angular acceleration of the mechanical arm;
Figure BDA0002095417580000084
miis the mass of the ith rod (i ═ 1,2,3), LiLength of ith rod (i ═ 1,2,3), liLength from ith rod centroid to previous joint (i ═ 1,2,3), JiMoment of inertia (i ═ 1,2,3) for the ith rod; hi∈R3×1(i is 1,2,3) is the terms of the coriolis force and the centrifugal force,
Figure BDA0002095417580000091
τ＝(0,τ2,τ3)Tis a joint torque vector, wherein the output torque of the passive joint is 0,
Figure BDA0002095417580000092
the ith joint angular velocity of the mechanical arm;
the complete system in mechanics means that all constraint conditions of a system can be directly changed into constraint without coordinate derivatives through an integral mode, the motion of the complete system is limited on a smooth hypersurface, in fact, most mechanical systems can be regarded as incomplete systems, including a first-order incomplete system with incomparable speed and a second-order incomplete system with incomparable acceleration, and the integrable conditions are specifically defined as follows:
definition 1: the first integrable condition: the moment generated by the potential energy term is a constant; the under-actuated joint variables do not appear in the inertial matrix;
definition 2: the second integrable condition is as follows: a first integrable condition is established; the under-actuated term null-space distribution in the inertial matrix is involutive;
according to the above definition, the planar PAA type under-actuated robot is a first-order incomplete system, and the planar PA type robot is an complete system; based on the idea of model order reduction, the control process is divided into two stages, the initial angle of the third joint is maintained to be unchanged in the first stage, and the second joint is controlled to reach the target angle; in the second stage, the target angle of the second joint is kept unchanged, and the third joint is controlled to reach the target angle; the line where underactuation is located is extracted in equation (1), and equation (4) is substituted to obtain:
Figure BDA0002095417580000093
by using fractional integration, equation (5) integrates time t to obtain:
Figure BDA0002095417580000094
the angle is kept constant in each control stage, and the angular velocity of the joint can be approximately considered
Figure BDA0002095417580000095
Zero, therefore, the integral is determined again across equation (6), simplifying to obtain:
Figure BDA0002095417580000101
wherein q is1 1(0) Representing the initial angle of the passive rod in the first stage, q2(0) And q is3(0) Representing the initial angles, D, of the second and third bars, respectively1，E1，G1，g1For the intermediate term of the reduction process of equation (7),
D1＝(8a3a5-4a2a6-4a1a6-4a4a6)cosq2(0)-a1 2-a2 2-4a6 2cos2q2(0)-a4 2+4a5 2+4a3 2-2a1a2-2a1a4-2a2a4
Figure BDA0002095417580000102
G1＝(a1+a2+a4+2a6cosq2(0)-2a5cosq2(0)-2a3)(cosq2-1)+2a5sinq2(0)sinq2
Figure BDA0002095417580000103
similarly, the second joint angle is maintained in the second stage of control, i.e., the joint angular velocity can be approximated
Figure BDA0002095417580000107
And (4) obtaining the second stage joint angle constraint by determining the integral at the two ends of the equation (6) again.
102, analyzing the reachable space of the under-actuated mechanical arm based on the angle integral constraint relation of the under-actuated mechanical arm, and calculating the target angle of the actuated joint by taking the difference value between the tail end of the mechanical arm and the target position as a fitness function of the particle swarm algorithm;
specifically, the working space of the robot arm refers to a set of spatial positions that can be reached by the end effector of the robot arm, as shown in fig. 2, the working domain of the robot arm is denoted as a (p), and then the mapping between each joint angle of the robot arm and the working reachable space can be expressed as:
A(p)＝{p(θ)|θ∈Q} (8)
wherein Q is the constraint space of joint angle to the angle constraint of joint spacing angle as the arm, promptly:
Figure BDA0002095417580000104
in the formula (I), the compound is shown in the specification,
Figure BDA0002095417580000105
and
Figure BDA0002095417580000106
denotes the lower and upper limits of joint movement, thetanIndicating the nth joint angle. In addition, angle constraint exists between the active joint and the passive joint of the PA type mechanical arm, and a joint angle constraint space is added; the essence of using Monte Carlo method to calculate the working space of multi-joint mechanical arm is that joint variables are assigned a certain number of coincidence relations through uniform distributionChanging the required random quantity, then combining the variables of each joint, and calculating the coordinate values of the end point of the end effector of the mechanical arm by forward kinematics recursion of the mechanical arm, wherein the set formed by the coordinate values is the reachable space of the tail end of the mechanical arm;
according to kinematics, as shown in fig. 3, the cartesian coordinates of the end position of a planar PAA type under-actuated manipulator can be expressed as:
Figure BDA0002095417580000111
wherein (X, Y) are cartesian coordinates of the end expectation; solving the angle of the driving rod according to the target coordinates of the end point of the system, which is essentially the inverse kinematics solution of the system; numerical methods are essentially guesses and iterations until the error is small enough, or until it is considered to be abandoned; selecting a difference value between the tail end coordinate of the mechanical arm and an expected position as an evaluation function, wherein the evaluation function can enable the particle swarm to continuously move to an optimal area in a solution space, and further a target angle of the driving rod is obtained;
the particle swarm optimization algorithm comprises the following steps:
1) randomly generating an initial population of m particles, initializing the speed and position of each particle, and giving an inertia weight omega, learning factors c1 and c2, pbest and gbest;
2) calculating the fitness of each particle;
3) updating the flight position and the flight speed of each particle according to the equations (11) and (12);
vi＝vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbesti-xi) (11)
xi＝xi+vi (12)
4) if the fitness of the particles is superior to the original individual extreme value pbest, the current fitness is set as pbest, and the optimal individual extreme value is selected as a group extreme value gbest;
5) and judging whether the algorithm reaches a termination condition. If yes, outputting a result; otherwise, performing a loop part from 1 to M, wherein M is the maximum iteration number;
and calculating the target angles of the two driving joints by using a particle swarm algorithm to lay a foundation for subsequent position control.
103, providing a planar three-degree-of-freedom underactuated mechanical arm sectional position control method based on a fuzzy function and a Lyapunov function, designing a fuzzy controller to enable a second joint to reach a target angle in a first stage, and solving a third joint control law through the Lyapunov function to enable the third joint control law to keep an initial state; in the second stage, designing a fuzzy controller to enable the third joint to reach the target angle, and solving a second joint control law through a Lyapunov function to enable the second joint to keep the target angle, wherein the method comprises the following steps:
first, the kinetic model is rewritten into an affine nonlinear system form as follows:
Figure BDA0002095417580000121
wherein F, G are both non-linear functions with respect to the state variable x,
Figure BDA0002095417580000122
Figure BDA0002095417580000123
[G1 G2 G3]T＝-M-1(q); according to the definition of the lyapunov function and the stable control target of the mechanical arm tail end at the target point, the constructed function ensures that the angle of the driving connecting rod finally tends to the target angle, and simultaneously, the angular speed of the driving connecting rod is finally converged to zero, so that the lyapunov function of the third connecting rod is constructed as follows:
Figure BDA0002095417580000124
wherein, V3(x) For a semi-positive definite function, the above equation is derived and the system state equation (12) is substituted to obtain:
Figure BDA0002095417580000125
wherein a isa22For the second row and second column entries in the G matrix, the control law can be found according to the following equation:
τ3＝(-x5+x5(0)-F3-μx5-aa22τ2)/aa22 (16)
where μ is a constant greater than 0, for adjusting the convergence rate; x is the number of5(0) Is the initial setting of the third joint angle. It can be seen that the output torque of the third joint changes with the output torque of the second joint, and the third joint is stabilized at the set initial angle regardless of the change in the angle of the second joint. Designing a fuzzy control law of a second connecting rod, taking a joint angle error and an error change rate as fuzzy system input, taking a moment of a second joint as output, taking a joint angle and joint angular velocity membership function as shown in figures 4 and 5, performing defuzzification by adopting a gravity center method, and when the second joint angle reaches a target position and the angular velocity is 0, controlling a strategy to enter a second stage; similar to the first stage, respectively constructing a Lyapunov function of the second joint, keeping the initial angle of the second joint unchanged, and designing a fuzzy controller to realize the control target of the third joint; when the angles of the driving rods are respectively stabilized at respective target angles, the angle control of the under-actuated connecting rod is jointly realized according to the constraint relation among the angles, and finally the position control of the PAA type mechanical arm is realized;
according to the method, under the condition that the steady-state result is not large, the overshoot and the steady-state time are obviously reduced, the control elements can be better optimized, and the mechanical arm can obtain better dynamic performance.
In specific implementation, the mechanical arm dynamics parameters and the particle swarm algorithm parameters are shown in the following 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
the fuzzy rule table in the fuzzy controller is 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, the initial configuration q of the robotic arminit＝[0 0 0](ii) a Selecting a target position as (1-0.5) by taking the base position as a coordinate origin, and solving the corresponding driving joint angle as [ 3.0117-1.7322 ] by a particle swarm algorithm]In radians. The two-stage control curve and the active joint torque output curve are shown in FIG. 7 and FIG. 8, and can be used in the reachable spaceWithin the range of (a), the proposed control method can effectively complete position control for any initial configuration, and the end position conforms to the error range. The method realizes the position control of a given target in limited time, and improves the performances of the tail end position, overshoot and steady-state time of the mechanical arm; specifically, under the condition that the steady-state result is not large, the overshoot and the steady-state time are obviously reduced, and the control elements can be better optimized, so that the mechanical arm obtains better dynamic performance.
The above-mentioned contents are only for illustrating the technical idea of the present invention, and the protection scope of the present invention is not limited thereby, and any modification made on the basis of the technical idea of the present invention falls within the protection scope of the claims of the present invention.

Claims (1)

1. A planar under-actuated mechanical arm position control method based on fuzzy control and a Lyapunov function is characterized by comprising the following steps:
(1) establishing a planar three-degree-of-freedom passive-active (PAA type) under-actuated mechanical arm dynamic model, and analyzing an integral constraint relation between angles and angular velocities of each joint, wherein the method specifically comprises the following steps:
the plane three-degree-of-freedom passive-active (PAA type) under-actuated mechanical arm is not influenced by gravity, the passive joint can rotate freely without brake moment and friction force, a research object is simplified into a rigid body structure, and a dynamic model is established according to a Lagrange equation as follows:
Figure FDA0002730268390000011
wherein M isij∈R3×3(i, j-1, 2,3) specifically,
Figure FDA0002730268390000012
ak(k ═ 1, 2.., 6) is a structural parameter of the system, q is a structural parameter of the systemi(i is 1,2,3) is the ith joint angle of the mechanical arm,
Figure FDA0002730268390000013
the ith joint angular acceleration of the mechanical arm;
Figure FDA0002730268390000014
miis the mass of the ith rod (i ═ 1,2,3), LiLength of ith rod (i ═ 1,2,3), liLength from ith rod centroid to previous joint (i ═ 1,2,3), JiMoment of inertia (i ═ 1,2,3) for the ith rod; hi∈R3×1(i is 1,2,3) is the terms of the coriolis force and the centrifugal force,
Figure FDA0002730268390000021
τ＝(0,τ2,τ3)Tis a joint torque vector, wherein the output torque of the passive joint is 0,
Figure FDA0002730268390000022
the ith joint angular velocity of the mechanical arm;
the plane PAA type under-actuated mechanical arm is a first-order incomplete system, and the plane PA type mechanical arm is an complete system; based on the idea of model order reduction, the control process is divided into two stages, the initial angle of the third joint is maintained to be unchanged in the first stage, and the second joint is controlled to reach the target angle; in the second stage, the target angle of the second joint is kept unchanged, and the third joint is controlled to reach the target angle; the line where underactuation is located is extracted in equation (1), and equation (4) is substituted to obtain:
Figure FDA0002730268390000023
by using fractional integration, equation (5) integrates time t to obtain:
Figure FDA0002730268390000024
the angle is kept constant in each control stage, and the angular velocity of the joint can be approximately considered
Figure FDA0002730268390000025
Zero, therefore, the integral is determined again across equation (6), simplifying to obtain:
Figure FDA0002730268390000026
wherein q is1 1(0) Representing the initial angle of the passive rod in the first stage, q2(0) And q is3(0) Representing the initial angles, D, of the second and third bars, respectively1,E1,G1,g1For the intermediate term of the reduction process of equation (7),
D1＝(8a3a5-4a2a6-4a1a6-4a4a6)cosq2(0)-a1 2-a2 2-4a6 2cos2q2(0)-a4 2+4a5 2+4a3 2-2a1a2-2a1a4-2a2a4
Figure FDA0002730268390000027
G1＝(a1+a2+a4+2a6cosq2(0)-2a5cosq2(0)-2a3)(cosq2-1)+2a5sinq2(0)sinq2
Figure FDA0002730268390000031
similarly, the second joint angle is maintained in the second stage of control, i.e., the joint angular velocity can be approximated
Figure FDA0002730268390000032
If the value is zero, the integral is determined again at the two ends of the formula (6) to obtain the joint angle constraint at the second stage;
(2) analyzing the reachable space of the joint, and calculating the target angle of each driving joint by using a particle swarm algorithm, specifically comprising the following steps:
calculating a target angle of a driving joint by taking a difference value between the tail end of the mechanical arm and a target position as a fitness function of the particle swarm algorithm;
the working space of the mechanical arm refers to a set of spatial positions that can be reached by the end effector of the mechanical arm, the working domain of the mechanical arm is denoted as a (p), and then the mapping of each joint angle of the mechanical arm and the working reachable space can be expressed as:
A(p)＝{p(θ)|θ∈Q} (8)
wherein Q is the constraint space of joint angle to the angle constraint of joint spacing angle as the arm, promptly:
Figure FDA0002730268390000033
in the formula (I), the compound is shown in the specification,
Figure FDA0002730268390000034
and
Figure FDA0002730268390000035
denotes the lower and upper limits of each joint motion, θnRepresents the nth joint angle; in addition, angle constraint exists between the active joint and the passive joint of the PA type mechanical arm, and a joint angle constraint space is added; calculating the working space of the multi-joint mechanical arm by using a Monte Carlo method through uniform divisionDistributing a certain amount of random quantities meeting the joint change requirement, then combining the joint variables, and calculating the coordinate values of the end point of the end effector of the mechanical arm by using the forward kinematics of the mechanical arm in a recursion manner, wherein the set formed by the coordinate values is the reachable space of the tail end of the mechanical arm;
the cartesian coordinates of the end position of the planar PAA type under-actuated manipulator can be expressed as:
Figure FDA0002730268390000036
wherein (X, Y) are cartesian coordinates of the end expectation; selecting a difference value between the tail end coordinate of the mechanical arm and an expected position as an evaluation function, wherein the evaluation function can enable the particle swarm to continuously move to an optimal area in a solution space, and further a target angle of the driving rod is obtained;
the particle swarm optimization algorithm comprises the following steps:
1) randomly generating an initial population of m particles, initializing the speed and position of each particle, and giving an inertia weight omega, learning factors c1 and c2, pbest and gbest;
2) calculating the fitness of each particle;
3) updating the flight position and the flight speed of each particle according to the equations (11) and (12);
vi＝vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbesti-xi) (11)
xi＝xi+vi (12)
4) if the fitness of the particles is superior to the original individual extreme value pbest, the current fitness is set as pbest, and the optimal individual extreme value is selected as a group extreme value gbest;
5) judging whether the algorithm reaches a termination condition, if so, outputting a result; otherwise, performing a loop part from 1 to M, wherein M is the maximum iteration number;
calculating target angles of the two driving joints by using a particle swarm algorithm to lay a foundation for subsequent position control;
(3) designing a segmentation position control law based on fuzzy control and a Lyapunov function, designing a fuzzy controller to enable a second joint to reach a target angle in a first stage, and designing the Lyapunov function to enable a third joint to keep an initial state; and designing a fuzzy controller at the second stage to enable the third joint to reach a target angle, designing a Lyapunov function to enable the second joint to keep the angle unchanged, and realizing position control, wherein the method specifically comprises the following steps:
first, the kinetic model is rewritten into an affine nonlinear system form as follows:
Figure FDA0002730268390000051
wherein F, G are both non-linear functions with respect to the state variable x,
Figure FDA0002730268390000054
[G1G2 G3]T＝-M-1(q); according to the definition of the lyapunov function and the stable control target of the mechanical arm tail end at the target point, the constructed function ensures that the angle of the driving connecting rod finally tends to the target angle, and simultaneously, the angular speed of the driving connecting rod is finally converged to zero, so that the lyapunov function of the third connecting rod is constructed as follows:
Figure FDA0002730268390000052
wherein, V3(x) For a semi-positive definite function, the above equation is derived and the system state equation (12) is substituted to obtain:
Figure FDA0002730268390000053
wherein a is22For the second row and second column entries in the G matrix, the control law can be found according to the following equation:
τ3＝(-x5+x5(0)-F3-μx5-a22τ2)/a22 (16)
where μ is a constant greater than 0, for adjusting the convergence rate; x is the number of5(0) Is the initial set value of the third joint angle; it can be seen that the output torque of the third joint changes with the output torque of the second joint, and the third joint is stabilized at the set initial angle no matter how the angle of the second joint changes; designing a fuzzy control law of a second connecting rod, taking the joint angle error and the error change rate as fuzzy system input, taking the moment of a second joint as output, performing defuzzification by adopting a gravity center method, and entering a second stage by a control strategy when the angle of the second joint reaches a target position and the angular speed is 0; similar to the first stage, respectively constructing a Lyapunov function of the second joint, keeping the initial angle of the second joint unchanged, and designing a fuzzy controller to realize the control target of the third joint; when the angles of the driving rods are respectively stabilized at respective target angles, the angle control of the under-actuated connecting rod is jointly realized according to the constraint relation among the angles, and finally the position control of the PAA type mechanical arm is realized.
CN201910517174.8A 2019-06-14 2019-06-14 Planar under-actuated mechanical arm position control method based on fuzzy control and Lyapunov function Expired - Fee Related CN110076783B (en)

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 CN110076783A (en) 2019-08-02
CN110076783B true 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)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021026779A1 (en) * 2019-08-13 2021-02-18 深圳市大疆创新科技有限公司 Cradle head control method and device, cradle head and storage medium
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
CN113296401B (en) * 2021-05-17 2022-04-19 哈尔滨工业大学 Direct obstacle avoidance tracking control method based on switching strategy and storage medium
CN115616922B (en) * 2022-12-19 2023-03-21 安徽大学 Time coverage control method for heterogeneous mobile robot cluster
CN116512286B (en) * 2023-04-23 2023-11-14 九众九机器人有限公司 Six-degree-of-freedom stamping robot and stamping method thereof
CN117047782B (en) * 2023-10-11 2023-12-08 中建四局安装工程有限公司 Control method and device suitable for three-joint manipulator, terminal and medium

Citations (5)

* Cited by examiner, † Cited by third party
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

Patent Citations (5)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
Two-Stage Switching Hybrid Control Method Based on Improved PSO for Planar Three-Link Under-Actuated Manipulator;X. Gao,etc;《IEEE Access》;IEEE;20190610;第7卷;第76263-76273页 *

Also Published As

Publication number Publication date
CN110076783A (en) 2019-08-02

Similar Documents

Publication Publication Date Title
CN110076783B (en) Planar under-actuated mechanical arm position control method based on fuzzy control and Lyapunov function
CN110275436B (en) RBF neural network self-adaptive control method of multi-single-arm manipulator
CN111319036B (en) Self-adaptive algorithm-based mobile mechanical arm position/force active disturbance rejection control method
CN110442020B (en) Novel fault-tolerant control method based on whale optimization algorithm
Korayem et al. Maximum load determination of nonholonomic mobile manipulator using hierarchical optimal control
CN107263483B (en) The control method for coordinating of two degrees of freedom articulated robot track
CN112000096A (en) Differential AGV trajectory tracking control method based on sparrow search algorithm
Kapnopoulos et al. A cooperative particle swarm optimization approach for tuning an MPC-based quadrotor trajectory tracking scheme
CN109262612B (en) Under-actuated mechanical arm joint angle optimization method based on improved particle swarm optimization
Mohd Basri et al. Design and optimization of backstepping controller for an underactuated autonomous quadrotor unmanned aerial vehicle
CN112936286B (en) Self-adaptive consistency tracking control method and system for multi-flexible mechanical arm system
Sadrnia et al. Design PID Estimator Fuzzy plus Backstepping to Control of Uncertain Continuum Robot
Basri et al. Nonlinear control of an autonomous quadrotor unmanned aerial vehicle using backstepping controller optimized by particle swarm optimization
Lai et al. A simple and quick control strategy for a class of first-order nonholonomic manipulator
Tang et al. Two recurrent neural networks for local joint torque optimization of kinematically redundant manipulators
Jun-Pei et al. Neural network control of space manipulator based on dynamic model and disturbance observer
Huang et al. Distributed flocking control of quad-rotor UAVs with obstacle avoidance under the parallel-triggered scheme
CN112947501B (en) Multi-AUV hybrid formation method based on improved artificial potential field method and state switching
CN111007848B (en) Multi-agent cooperative operation control method based on bounded space
CN112016162A (en) Four-rotor unmanned aerial vehicle PID controller parameter optimization method
Vladareanu et al. Fuzzy dynamic modeling for walking modular robot control
Basri et al. Backstepping controller with intelligent parameters selection for stabilization of quadrotor helicopter
Mehdi et al. Position/force control optimized by particle swarm intelligence for constrained robotic manipulators
Nurmaini et al. Enhancement of the fuzzy control response with particle swarm optimization in mobile robot system
Kalshetti et al. Self-adaptive grey wolf optimization based adaptive fuzzy aided sliding mode control for robotic manipulator

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