CN114800528A - Mobile manipulator repetitive motion planning for non-convex anti-noise type return-to-zero neural network - Google Patents
Mobile manipulator repetitive motion planning for non-convex anti-noise type return-to-zero neural network Download PDFInfo
- Publication number
- CN114800528A CN114800528A CN202210632480.8A CN202210632480A CN114800528A CN 114800528 A CN114800528 A CN 114800528A CN 202210632480 A CN202210632480 A CN 202210632480A CN 114800528 A CN114800528 A CN 114800528A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- convex
- omnidirectional
- neural network
- constraint
- 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.)
- Pending
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 70
- 230000003252 repetitive effect Effects 0.000 title claims abstract description 40
- 238000013528 artificial neural network Methods 0.000 title claims abstract description 31
- 238000000034 method Methods 0.000 claims abstract description 32
- 230000008859 change Effects 0.000 claims abstract description 30
- 230000008569 process Effects 0.000 claims abstract description 15
- 230000004913 activation Effects 0.000 claims description 18
- 238000003062 neural network model Methods 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000013507 mapping Methods 0.000 claims description 6
- 238000013461 design Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims 1
- 230000009466 transformation Effects 0.000 abstract description 15
- 230000006870 function Effects 0.000 description 40
- 239000012636 effector Substances 0.000 description 12
- 238000010586 diagram Methods 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000011160 research Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000013020 final formulation Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000001537 neural effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000035939 shock Effects 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
Images
Classifications
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J5/00—Manipulators mounted on wheels or on carriages
- B25J5/007—Manipulators mounted on wheels or on carriages mounted on wheels
-
- 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
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a repetitive motion planning method for a movable mechanical arm of a non-convex anti-noise return-to-zero neural network, which comprises the following steps: a. obtaining an integral mobile mechanical arm kinematic equation of the mobile platform and the mechanical arm based on integrity constraint and space coordinate transformation; b. constructing time-varying quadratic programming of a plurality of subtasks of joint and wheel speed limitation, track tracking and repeated motion aiming at repeated motion of the mobile mechanical arm; c. and providing a non-convex anti-noise type return-to-zero neural network for solving a quadratic programming model of the mobile mechanical arm in noise disturbance. The non-convex anti-noise type return-to-zero neural network controller is designed based on the difference value between the given expected track function and the actual motion track as an error function, so that noise interference in the track tracking process of the mobile mechanical arm is inhibited, and the robustness is enhanced. The non-convex set restricts the error change rate from a larger non-convex set to a smaller non-convex set through twice restriction, so that the stability is enhanced, and the track tracking task is finally completed.
Description
Technical Field
The invention relates to the field of mobile robots, in particular to a four-wheel mobile mechanical arm trajectory tracking planning method of a repetitive motion anti-noise type return-to-zero neural network.
Background
With the continuous progress of artificial intelligence technology, the intelligent robot industry is developed vigorously. The intelligent robot is a robot system which integrates the aspects of perception, thinking, effect and the like and comprehensively simulates people, and can replace human beings to greatly expand the body and hands in various fields. When the mobile mechanical arm works in a dynamic and unknown complex environment, the system has complete autonomy, namely the system has sensing capability, planning capability, maneuvering capability and coordination capability, and in the field of research of the mobile mechanical arm, the problems of trajectory planning, motion control, cooperative control and the like need to be solved. According to different control targets, motion control of the mobile mechanical arm can be divided into three types of point stabilization, path following and trajectory tracking, wherein trajectory tracking repetitive motion control is a hotspot and difficulty of research in the field of mobile mechanical arm control in recent years.
At present, most of theoretical research on mobile mechanical arms is based on two-wheel mobile mechanical arms or three-wheel mobile mechanical arms, robot models are based on dynamic modeling, the dynamic modeling of four-wheel mobile mechanical arms is complex, and a dynamic model of a mobile platform and a dynamic model of the mechanical arms need to be analyzed simultaneously. The two models are difficult to integrate into an integrated model, so most researchers adopt two control algorithms to respectively control the mobile platform and the mechanical arm, and the mobile platform and the mechanical arm are difficult to realize cooperative control. The mobile platform and the mechanical arm overall kinematic model can complete the cooperative control task more easily. In addition, kinematic modeling is relatively simple compared to dynamic modeling of the system. At present, most of the establishment of the overall kinematic models of the mobile platform and the mechanical arm based on non-integrity constraint leads to the controllable degree of freedom of the mobile mechanical arm being less than the overall degree of freedom of the mobile mechanical arm. However, an omni-directional mobile robotic arm established based on integrity constraints can move in all directions, such as sideways movement occurs, and the total degree of freedom of the mobile robotic arm is equal to the controllable degree of freedom. Therefore, the invention establishes a kinematics model of the omnidirectional Maclam wheel mobile mechanical arm based on integrity constraint, integrates the omnidirectional mobile platform and the mechanical arm in a world-based coordinate system through space coordinate transformation, and provides the anti-noise non-convex omnidirectional four-wheel mobile mechanical arm track repetitive motion method. Firstly, the anti-noise return-to-zero neural network can solve the control problem of the mechanical arm under the condition of external noise interference, and the robustness of completing repetitive motion cooperative control is enhanced. Secondly, the existing repetitive motion planning scheme for kinematic control of the mobile mechanical arm applies convex boundary constraint to joint limitation, and in non-convex mapping under the bounded constraint, the convex boundary constraint cannot well describe the actual situation in the application of the robot. The activation function is a non-convex activation function and is more consistent with the actual situation. For example, the output joint velocity or joint acceleration of the motor that moves the ith joint of the robot arm to perform the task may be discontinuous, i.e. the pulses per second used to control the motor in the robot are represented in a discrete form, which is obviously a non-convex mapping situation, and the convex boundary constraint does not meet the practical situation. On the other hand, the non-convex set of the non-convex activation function has twice constraints on the error change rate, the first constraint is that the error change rate is constrained in a specified smaller set when the repetitive motion task starts, the situation that the moving mechanical arm cannot complete repetitive motion cooperative control due to large change of the starting error is avoided, the non-convex set constrains the error change rate in a smaller range set again along with the progress of the repetitive motion task, and the non-convex set of the non-convex activation function finally and stably constrains the error change rate in the smaller set through twice constraints, so that the moving mechanical arm is guaranteed to stably complete the repetitive motion cooperative control. The error change rate reflects error change, the smaller the error change is, the stability of the proposed algorithm is indicated, and the mobile mechanical arm can stably complete the repetitive motion cooperative control. The non-convex anti-noise return-to-zero neural network is utilized to realize accurate track tracking repetitive motion cooperative control of the omnidirectional four-wheel mobile mechanical arm.
Disclosure of Invention
The invention discloses a repetitive motion planning method for a moving mechanical arm of a non-convex anti-noise return-to-zero neural network, which is characterized in that an overall kinematic equation of four-wheel moving mechanical arm is established under a world coordinate system based on integrity constraint, a closed expected track equation is designed in a reachable space range of the moving mechanical arm, a vector type error function is defined based on a difference value between a motion track and an actual motion track function, and a differential equation of a constructed error function e (t) meets the requirement
Φ Ω (. cndot.) represents a non-convex activation function. A smaller absolute value of the error function represents a better completion of the trajectory tracking task. The method comprises the steps of constructing a plurality of subtasks of trajectory tracking, repeated movement, joint and wheel speed limitation into time-varying quadratic programming of equality constraint and inequality constraint, constructing a time-varying quadratic programming problem of solving equality constraint and inequality constraint by using a Lagrangian function, converting the time-varying quadratic programming problem with equality constraint and inequality constraint into a time-varying extreme value problem of solving the Lagrangian function, and obtaining a vector type error function by using Karush-Kuhn-Tucker (KKT) conditions. And solving a plurality of subtask time-varying quadratic programming repetitive motion cooperative control strategies by using the anti-noise return-to-zero neural model.
The technical scheme of the invention is as follows by combining the attached drawings of the specification:
the anti-noise type return-to-zero neural network control method based on the non-convex edge constraint repeated motion planning of the omnidirectional four-wheel mobile mechanical arm specifically comprises the following steps:
s1: acquiring initial angle data of four wheels of an omnidirectional four-wheel mobile mechanical arm and initial angle data of a four-degree-of-freedom mechanical arm;
s2: designing an expected trajectory equation of the omnidirectional four-wheel mobile mechanical arm according to requirements;
s3: obtaining a kinematics equation of the four-degree-of-freedom mechanical arm under a base coordinate system through space coordinate transformation, establishing a kinematics model of the omnidirectional moving platform based on integrity constraint, and obtaining an overall kinematics equation of the omnidirectional moving mechanical arm based on a world coordinate system through coordinate transformation by combining the kinematics model of the omnidirectional moving platform and the kinematics model of the mechanical arm;
s4: converting trajectory tracking, repeated motion and joint constraint into a time-varying quadratic programming problem of equality and inequality constraint;
s5: converting the inequality constraint of S4 into an equality constraint, and constructing a kinematics model of the omnidirectional four-wheel mobile mechanical arm by combining an omnidirectional four-wheel mobile mechanical arm repetitive motion scheme and an anti-noise type return-to-zero neural network model to obtain an anti-noise type return-to-zero neural network controller, so as to complete track tracking and repetitive tasks of the omnidirectional four-wheel mobile mechanical arm;
s6: the trajectory tracking problem of the moving mechanical arm is solved based on the neurodynamic equation in step S5.
The specific process of step S1 is:
in the experiment, hardware parameters of the four-wheel mobile mechanical arm are required to be referred, the height of the mobile platform is measured through a meter ruler, and the working range of each mechanical arm is measured under the condition of power failure. Wherein the parameters of each joint are respectively as follows: the working range of the base of the shaft 1 is from +90 degrees to-90 degrees, and the maximum speed is 320 degrees/s; the working range of the large arm of the shaft 2 is 0-85 degrees, and the maximum speed is 320 degrees/s; the working range of the small arm of the shaft 3 is from minus 10 degrees to plus 95 degrees, and the maximum speed is 320 degrees/s; the shaft 4 rotates within the working range of +90 degrees to-90 degrees, and the maximum speed is 480 degrees/s;
the specific process of step S2 is:
the desired trajectory of the end effector of the four-wheel mobile robot arm is designed based on the measurement values in step S1, ensuring that it cannot exceed the reach of the joints of the mobile robot arm. Wherein the mathematical expression of the desired trajectory is as follows:
r xd ＝0.4*cos(4*π*(sin(0.5*π*t/10)) 2 +π/6)-0.4*cos(π/6)
r xd ＝0.4*sin(2*π*(sin(0.5*π*t/10)) 2 +π/6)+0.4*sin(π/6)
r xd ＝0.2362
r d ＝[r xd ；r xd ；r xd ]
in the formula, r xd Is r d Projecting the curve on an x plane; r is yd Is r d At y is flatA surface projection curve; r is zd Is r d The curve is projected in the z plane.
The specific process of step S3 is:
s301: in order to describe the relative position and direction relationship of the links of the mobile mechanical arm, a coordinate system needs to be established on each link according to the joint structure of the mechanical arm as follows:
i-1 T i ＝Rot(x,α i-1 )Trans(x,a i-1 )Rot(z,θ i )Trans(z,d i )
establishing a kinematic model of the mobile mechanical arm by using a D-H method, and performing homogeneous transformation on a connecting rod coordinate system { i } relative to { i-1} i-1 T i So-called connecting rod change, in which the angle of rotation of the shaft alpha is involved i-1 Length of connecting rod a i-1 Offset distance d of connecting rod i And joint variable theta i And can therefore be decomposed into sub-transformation problems of the coordinate system i, each sub-transformation relying on only one link parameter.
The kinematic equation of the mechanical arm obtained by coordinate transformation is as follows:
wherein h is 1 Is the length of the connecting rod 1; h is 2 Is the length of the connecting rod 2; h is 3 Is the length of the connecting rod 3; h is 4 Is the length of the connecting rod 4; c. C 1 ＝cosφ 1 ，s 1 ＝sinφ 1 ，c 23 ＝cos(φ 2 +φ 3 )，s 23 ＝sin(φ 2 +φ 3 )；
S302: the moving platform selects Mecanum wheels as driving wheels, and a four-wheel full-drive mode is adopted in the aspect of power. Decomposing the motion of a Mecanum wheel chassis of a mobile platform into three independent variable descriptions; calculating the speed of the axle center position of each wheel; calculating the speed of the roller of the wheel contacting with the ground according to the result of the first step; and according to the result of the second step, calculating the real rotating speed of the wheel to obtain a positive solution of the four-wheel omnidirectional kinematics model:
wherein a is the distance from the middle point of the front and rear wheels to the front and rear wheels, b is the distance from the middle point of the front and rear wheels to the middle point of the four wheels,
the direction representing the X-axis motion, i.e., the left-right direction, is defined as positive to the right,
indicating the direction of Y-axis motion, i.e., the fore-aft direction, is defined forward as positive,
the angular velocity representing the rotation of the yaw axis, defined as positive counterclockwise, is the velocity of the geometric center (the diagonal of the rectangle) of the four wheels.
Representing the speed of each wheel.
S303: through the transformation matrix from the base coordinate system to the world coordinate system, the overall kinematic equation of the mobile robot arm with respect to the world coordinate system can be obtained:
and (3) obtaining a kinematic equation of the movement robot arm speed level under the world coordinate system by deriving the time t by the formula as follows:
wherein, the Jacobian matrix
ν＝[φ T ,α] T
Finally, the following simplified speed level kinematic equation is formed:
wherein,
and the angle vector of the mobile mechanical arm is represented, and comprises the rotation angle of the wheel of the mobile platform and the rotation angle of each joint of the mechanical arm.
The specific process of step S4 is:
constructing a time-varying equation and inequality constrained quadratic programming scheme for multiple subtasks of track tracking, repetitive motion and joint and wheel speed constraint, constructing an omnidirectional moving mechanical arm repetitive motion model according to the omnidirectional four-wheel moving mechanical arm repetitive motion scheme and an anti-noise type zeroing neural network control method, and summarizing the multiple subtasks of the omnidirectional four-wheel moving mechanical arm speed level overall kinematics model, joint constraint, track tracking and repetitive motion into an equation and inequality constrained time-varying quadratic programming as follows:
wherein c ═ μ 3 (p c -p c (0))；μ 2 (sin(α)-sinα(0))；μ 1 (φ-φ(0))]；Q＝[D,0；0,I]∈R (n +m)×(m+2) ，D＝[B；A cosα]∈R m×2 ；p c And alpha is the connecting point (X) of the omnidirectional moving platform and the mechanical arm respectively d ,Y d 0) and the heading angle of the mobile platform; p is a radical of c (0) And sin α (0) are each p c And an initial value of α; mu.s 1 ,μ 2 ,μ 3 Respectively are feedback coefficients; n and m are dimensional coefficients of the matrix; q. q.s T ＝Q T c and H ═ Q T Q is a coefficient matrix for ensuring that the omnidirectional mobile mechanical arm realizes repeated motion; upper label T A transpose operation of the representative matrix; xi + ，ξ - Representing the constraint boundaries of joint and wheel speed;
the specific process of step S5 is:
the inequality constraint time-varying quadratic programming problem is converted into an equality constraint time-varying quadratic programming as follows:
K(t)ξ(t)-d(t)+τ 2 (t)＝0.
in the formula tau 2 (t)∈R p ，τ 2 (t)＝τ(t)°τ(t)＝[τ 2 (t)]Is the relaxation coefficient;
the time-varying quadratic programming problem with equality and inequality constraint solving needs to be constructed, and the time-varying quadratic programming problem with equality and inequality constraint solving is converted into the problem of solving the extreme value of the Lagrangian function, so that the problem of solving the extreme value of the Lagrangian function is solved
Using the Karush-Kuhn-Tucker (KKT) condition, the following equation was obtained:
Z(t)y(t)＝π(t)
in the formula
π(t)＝[-q(t) ν(t) d(t) π(t)] T
According to a design formula of the anti-noise type return-to-zero neural network model, an error function of the system is as follows:
e(t)＝Z(t)y(t)-π(t)
in order to solve an accurate solution of the time-varying inverse kinematics, each term of the error function is required to approach zero, and the anti-noise type return-to-zero neural network kinetic equation is as follows:
wherein phi Ω (. cndot.) represents the activation function of the neural network, and the non-convex activation function Φ is selected Ω (X)＝arg min Y∈Ω ||Y-X|| 2 Where 0 ∈ Ω. Gamma is more than 0, lambda is more than 0, and the convergence speed of the system can be changed by adjusting the parameters. An integral term is introduced in the kinetic equation to eliminate noise.
Constructing a zeroing neural network dynamic equation with an interference term, wherein the specific mathematical expression is as follows:
wherein, eta (t) is a noise interference term, and external interference influencing the normal work of the robot always exists in the running process of the mobile robot. For example, constant external shock interference, environmental impact force, electromagnetic interference in a circuit system, and random noise in a signal transmission process, and these time-varying disturbances can have a great influence on the accuracy and stability of a model solving problem, so that a repetitive motion task fails. The anti-interference return-to-zero neural network solution of the omnidirectional four-wheel mobile mechanical arm with external interference can more accurately complete track tracking and repetitive motion tasks.
The specific process of step S6 is:
the situation of the change of the vehicle speed of the wheels of the moving platform and the rotation angle of each joint in the process of tracking the expected track of the omnidirectional four-wheel moving mechanical arm are solved through the kinematic equation, and the parameters obtained through calculation can be applied to each motor to adjust each variable to perform track tracking repetitive motion.
Compared with the prior art, the invention has the advantages that:
the method is characterized in that a global kinematics model of the omnidirectional four-wheel mobile mechanical arm is established based on integrity constraints, the track tracking, the repeated motion, the joint and wheel speed limitation are designed into a time-varying quadratic programming model of a plurality of subtask equality and inequality constraints, the inequality constraints are converted into equality constraints by adding non-negative terms, and a time-varying quadratic programming scheme of the equality and inequality constraints is converted into a time-varying quadratic programming model of the equality constraints. Characterized in that, firstly: in the traditional control of the mobile mechanical arm, a kinematic model of a system needs to be established, and the mobile platform and each joint mechanical arm need to be controlled respectively, however, in the invention, the kinematic modeling is carried out on the mobile platform and the mechanical arm respectively, so that the complex kinematic modeling is avoided, and the two are integrated into one system through space coordinate transformation, so that the cooperative control of the mobile mechanical arm is realized. Secondly, the method comprises the following steps: the omnidirectional mobile mechanical arm is established based on the integrity constraint, and different from the establishment of the mobile mechanical arm based on the non-integrity constraint, the omnidirectional mobile mechanical arm established based on the integrity constraint can move towards all directions, if the omnidirectional mobile mechanical arm moves laterally, and the total degree of freedom of the mobile mechanical arm is equal to the controllable degree of freedom. And the controllable degree of freedom of the omnidirectional mobile mechanical arm established based on the non-integrity constraint is smaller than the total degree of freedom of the mobile mechanical arm. Typically, the degree of freedom of the moving robot arm is 3, including the lateral axis, the longitudinal axis and the orientation. Thirdly, the method comprises the following steps: the invention designs an anti-noise return-to-zero neural network control algorithm to solve the problem of track tracking of a four-wheel moving mechanical arm and solve the control problem of the moving mechanical arm under the condition of external noise interference.
It is worth noting that all existing iterative motion planning schemes for kinematic control of a moving robot arm impose convex boundary constraints on joint limits, which in non-convex mapping under the bounded constraints may not well describe the reality in robotic applications. The activation function is a non-convex activation function and is more consistent with the actual situation. On the one hand, the output joint velocity or joint acceleration of the motor moving the ith joint performing the task of the robot arm may be discontinuous, i.e. the pulses per second used to control the motor in the robot are represented in a discrete form, which is obviously a non-convex mapping case. The detailed description is as follows: assuming that the motor outputs a joint speed of
Is discontinuous within a range wherein
And
representing the lower and upper limits of the motor output speed. Then there is a value
And is
In addition, there must be
And is
This indicates that the discontinuity is non-convex, and it is clear that convex constraints cannot be met. On the other hand, the non-convex set of the non-convex activation function has two constraints on the error change rate, wherein the first constraint is the error change at the beginning of the repeated motion taskThe rate is constrained within a specified smaller set such as [ -0.10.1 [ ]]And the problem that the moving mechanical arm cannot complete repeated motion cooperative control due to the initial error change is avoided. As the repetitive motion task progresses, the non-convex set of non-convex activation functions again constrains the error rate of change to a very small set of ranges, e.g., -0.0010.001]The non-convex set of the non-convex activation function finally constrains the error change rate to a very small set by twice constraining [ -0.0010.001]Therefore, the mobile mechanical arm is guaranteed to stably complete the repetitive motion cooperative control. The error change rate reflects error change, and the smaller the error change is, the more stable or robust the proposed algorithm is. Finally, the effectiveness of the algorithm is verified through simulation experiments.
Drawings
FIG. 1 is a schematic diagram of an anti-noise type return-to-zero neural network model for suppressing external time-varying disturbance by a non-convex projection function according to the present invention;
FIG. 2 is a point connection profile and course angle for a non-convex projection function repetitive motion scheme of the present invention;
FIG. 3 is an image of angular changes of each mechanical arm of an omnidirectional four-wheel mechanical arm end effector controlled by the non-convex projection function anti-noise type return-to-zero neural network model according to an expected track;
FIG. 4 is an image of the rotation angle change of each wheel of the omnidirectional four-wheel mobile mechanical arm end effector controlled by the non-convex projection function anti-noise type return-to-zero neural network model according to the present invention to track an expected track;
FIG. 5 is an image of angular velocity changes of each arm of an end effector of a four-wheeled omnidirectional mobile arm controlled by the non-convex projection function anti-noise type return-to-zero neural network model according to the present invention to track an expected trajectory;
FIG. 6 is an image of the change of the rotational angular velocity of each wheel of the four-wheel omni-directional mobile mechanical arm end effector controlled by the non-convex projection function anti-noise type return-to-zero neural network model according to the present invention to track the expected trajectory;
FIG. 7 is an error image of the non-convex projection function anti-noise type return-to-zero neural network model controlling the end effector of the four-wheel omni-directional mobile manipulator to track an expected trajectory according to the present invention;
FIG. 8 is an image of the error change rate of the non-convex projection function anti-noise type return-to-zero neural network model controlling the end effector of the four-wheel omni-directional mobile manipulator according to the present invention;
FIG. 9 is a top view image of a non-convex projection function anti-noise type return-to-zero neural network model controlling a four-wheel omni-directional mobile manipulator end effector trajectory tracking desired trajectory according to the present invention;
Detailed Description
In order to more clearly and completely describe the control method and the specific data processing and designing process thereof, the following description will be made in conjunction with the accompanying drawings, and those skilled in the art can implement the present invention according to the content described in the specification:
the invention discloses a repeated motion planning of a mobile mechanical arm of a non-convex anti-noise type return-to-zero neural network, wherein a schematic diagram of an anti-noise type return-to-zero neural network model is shown in figure 1, and the method specifically comprises the following steps:
s1: acquiring initial angle data of four wheels of an omnidirectional four-wheel mobile mechanical arm and initial angle data of a four-degree-of-freedom mechanical arm;
in step S1, hardware parameters of the four-wheel mobile robot arm to be referred to in the experiment are collected, the height of the mobile platform is measured by a meter ruler, and the working range of each robot arm is measured in case of power failure. Wherein the parameters of each joint are respectively as follows: the working range of the base of the shaft 1 is from +90 degrees to-90 degrees, and the maximum speed is 320 degrees/s; the working range of the large arm of the shaft 2 is 0-85 degrees, and the maximum speed is 320 degrees/s; the working range of the small arm of the shaft 3 is from minus 10 degrees to plus 95 degrees, and the maximum speed is 320 degrees/s; the shaft 4 rotates in the working range from +90 degrees to-90 degrees, and the maximum speed is 480 degrees/s.
S2: designing an expected trajectory equation of the omnidirectional four-wheel mobile mechanical arm according to requirements;
in step S2, the expected trajectory of the end effector of the four-wheel mobile robot arm is designed to ensure that it cannot exceed the reach of each joint of the mobile robot arm. Wherein the mathematical expression for the desired trajectory is:
r xd ＝0.4*cos(4*π*(sin(0.5*π*t/10)) 2 +π/6)-0.4*cos(π/6)
r xd ＝0.4*sin(2*π*(sin(0.5*π*t/10)) 2 +π/6)+0.4*sin(π/6)
r xd ＝0.2362
r d ＝[r xd ；r xd ；r xd ]
in the formula, r xd Is r d Projecting the curve on an x plane; r is yd Is r d Projecting the curve on a y plane; r is zd Is r d The curve is projected in the z plane.
S3: obtaining a kinematics equation of the four-degree-of-freedom mechanical arm under a base coordinate system through space coordinate transformation, establishing a kinematics model of the omnidirectional moving platform based on integrity constraint, and obtaining an overall kinematics equation of the omnidirectional moving mechanical arm based on a world coordinate system through coordinate transformation by combining the kinematics model of the omnidirectional moving platform and the kinematics model of the mechanical arm;
in step S3, a kinematics equation of the four-degree-of-freedom manipulator in a polar coordinate system is obtained through spatial coordinate transformation, an omnidirectional moving flat kinematics model is established based on integrity constraint, and an overall kinematics model of the omnidirectional moving manipulator in a world coordinate system is obtained through coordinate transformation. The specific process is as follows:
s301: according to the relative position and the directional relation of each connecting rod of the mobile mechanical arm, a coordinate system needs to be established on each connecting rod according to the joint structure of the mechanical arm as follows:
i-1 T i ＝Rot(x,α i-1 )Trans(x,a i-1 )Rot(z,θ i )Trans(z,d i )
establishing a kinematic model of the mobile mechanical arm by using a D-H method, and performing homogeneous transformation on a connecting rod coordinate system { i } relative to { i-1} i-1 T i So-called link change, in which the angle of rotation alpha of the shaft is involved i-1 Length of connecting rod a i-1 Offset distance d of connecting rod i And joint variable theta i And can therefore be decomposed into sub-transformation problems of the coordinate system i, each sub-transformation relying on only one link parameter.
The kinematic equation of the mechanical arm obtained through coordinate transformation is as follows:
wherein h is 1 Is the length of the connecting rod 1; h is 2 Is the length of the connecting rod 2; h is 3 Is the length of the connecting rod 3; h is 4 Is the length of the connecting rod 4; c. C 1 ＝cosφ 1 ，s 1 ＝sinφ 1 ，c 23 ＝cos(φ 2 +φ 3 )，s 23 ＝sin(φ 2 +φ 3 )；
S302: decomposing the motion of a Mecanum wheel chassis of the mobile platform into three independent variable descriptions; calculating the speed of the axle center position of each wheel; calculating the speed of the roller of the wheel contacting with the ground according to the result of the first step; and according to the result of the second step, calculating the real rotating speed of the wheel, and obtaining a positive solution of the four-wheel omnidirectional kinematics model as follows:
wherein a is the distance from the middle point of the front and rear wheels to the front and rear wheels, b is the distance from the middle point of the front and rear wheels to the middle point of the four wheels,
the direction representing the X-axis motion, i.e., the left-right direction, is defined as positive to the right,
indicating the direction of Y-axis motion, i.e., the fore-aft direction, is defined forward as positive,
the angular velocity representing the rotation of the yaw axis, defined as positive counterclockwise, is the velocity of the geometric center (the diagonal of the rectangle) of the four wheels.
Representing the speed of each wheel.
S303: according to the transformation matrix from the base coordinate system to the world coordinate system, the overall kinematic equation of the mobile mechanical arm about the world coordinate system can be obtained as follows:
and (3) obtaining a kinematic equation of the movement robot arm speed level under the world coordinate system by deriving the time t by the formula as follows:
wherein, the Jacobian matrix
ν＝[φ T ,α] T
The final formulation is the following simplified velocity level kinematic equation:
wherein,
and the angle vector of the mobile mechanical arm is represented, and comprises the rotation angle of the wheel of the mobile platform and the rotation angle of each joint of the mechanical arm.
S4: converting trajectory tracking, repeated motion and joint constraint into a time-varying quadratic programming problem of equality and inequality constraint;
in step S4, a time-varying equation and inequality constrained quadratic programming scheme is constructed for the multiple subtasks of trajectory tracking, repetitive motion, joint and wheel speed constraint, an omnidirectional moving mechanical arm repetitive motion kinematics model is constructed according to the orthogonal repetitive motion scheme of the omnidirectional four-wheel moving mechanical arm and the anti-noise return-to-zero neural network control method, and the multiple subtasks of the speed level global kinematics model, joint constraint, trajectory tracking and repetitive motion of the omnidirectional four-wheel moving mechanical arm are summarized as the following equation and inequality constrained time-varying quadratic programming:
wherein c ═ μ 3 (p c -p c (0))；μ 2 (sin(α)-sinα(0))；μ 1 (φ-φ(0))]；Q＝[D,0；0,I]∈R (n +m)×(m+2) ，D＝[B；Acosα]∈R m×2 ；p c And alpha is the connecting point (X) of the omnidirectional moving platform and the mechanical arm respectively d ,Y d 0) and the heading angle of the mobile platform; p is a radical of c (0) And sin α (0) are each p c And an initial value of α; mu.s 1 ,μ 2 ,μ 3 Respectively are feedback coefficients; n and m are dimensional coefficients of the matrix; q. q.s T ＝Q T c and H ═ Q T Q is a coefficient matrix for ensuring the omnidirectional mobile mechanical arm to realize repeated motion; upper label T A transpose operation of the representative matrix; xi + ，ξ - Representing the constraint of joint and wheel speed boundaries.
S5: converting the inequality constraint of S4 into an equality constraint, and constructing a kinematics model of the four-wheel mobile mechanical arm by combining an omnidirectional four-wheel mobile mechanical arm repetitive motion scheme and an anti-noise type return-to-zero neural network model to obtain an anti-noise type return-to-zero neural network controller, so as to complete track tracking and repetitive tasks of the omnidirectional four-wheel mobile mechanical arm;
in the step S5, firstly, the inequality-constrained time-varying quadratic programming problem is converted into an equality-constrained time-varying quadratic programming, and secondly, the time-varying quadratic programming problem is converted into a nonlinear equality online solving problem by using the constructed lagrangian function and the Karush-Kuhn-tucker (kkt) condition, so that a controller based on the anti-noise return-to-zero neural network is designed, and the cooperative control of the repetitive motion of the omnidirectional mobile mechanical arm in the noise environment is realized.
The inequality constraint time-varying quadratic programming problem is converted into an equality constraint time-varying quadratic programming as follows:
K(t)ξ(t)-d(t)+τ 2 (t)＝0.
in the formula tau 2 (t)∈R p ，τ 2 (t)＝τ(t)°τ(t)＝[τ 2 (t)]Is the relaxation coefficient;
the time-varying quadratic programming problem with equality and inequality constraint solving needs to be constructed, and the time-varying quadratic programming problem with equality and inequality constraint solving is converted into the problem of solving the extreme value of the Lagrangian function, so that the problem of solving the extreme value of the Lagrangian function is solved
Using the Karush-Kuhn-Tucker (KKT) condition, the following equation was obtained:
Z(t)y(t)＝π(t)
in the formula
π(t)＝[-q(t) ν(t) d(t) π(t)] T
According to a design formula of the anti-noise type return-to-zero neural network model, an error function of the system is as follows:
e(t)＝Z(t)y(t)-π(t)
in order to solve an accurate solution of the time-varying inverse kinematics, each term of the error function is required to approach zero, and the anti-noise type return-to-zero neural network kinetic equation is as follows:
wherein phi Ω (. represents the activation function of the neural network, selecting the non-convex activation function Φ Ω (X)＝arg min Y∈Ω ||Y-X|| 2 Where 0 ∈ Ω. Gamma is more than 0, lambda is more than 0, and the convergence speed of the system can be changed by adjusting the parameters. An integral term is introduced in the kinetic equation to eliminate noise.
Constructing a zeroing neural network dynamic equation with an interference term, wherein the specific mathematical expression is as follows:
wherein, eta (t) is a noise interference term, and external interference influencing the normal work of the robot always exists in the running process of the mobile robot.
S6: the trajectory tracking problem of the moving mechanical arm is solved based on the neurodynamic equation in step S5.
In step S5, the change of the vehicle speed of the wheels of the omnidirectional four-wheel mobile robot arm and the rotation angle of each joint during the process of tracking the expected trajectory are solved by the kinematic equation, and the calculated parameters can be applied to each motor to adjust each variable to perform trajectory tracking repetitive motion.
As shown in fig. 2-9 are allConnecting a section and a course angle, an angle change diagram, a wheel rotation angle change diagram, a mechanical arm angular velocity change diagram, a wheel rotation angular velocity change diagram, an error diagram of an end effector tracking expected track, an error change rate diagram of the end effector and a top view of the end effector track tracking expected track to the point of a moving mechanical arm repeated motion scheme under a non-convex projection function. The omnidirectional moving mechanical arm realizes good control of the omnidirectional moving mechanical arm under the control of a non-convex anti-noise return-to-zero neural network, particularly reflects that the expected track of the system is well tracked on the output track of a controller, and the minimum error can reach 10 -4 。
Claims (3)
1. The repeated motion planning method for the mobile mechanical arm of the non-convex anti-noise type return-to-zero neural network is characterized by comprising the following steps of:
s1: designing an expected trajectory equation of the omnidirectional four-wheel mobile mechanical arm according to requirements;
s2: giving an initial rotation angle of each wheel of the omnidirectional four-wheel mobile mechanical arm and an initial angle of the four-degree-of-freedom mechanical arm, and measuring the length and the width of the mobile platform;
s3: constructing an overall kinematics model of the mechanical arm of the omnidirectional four-wheel mobile platform;
s4: converting equality constraint and inequality constraint of track tracking, repeated motion and joint constraint into a time-varying quadratic programming model;
s5: and converting the inequality constraint of the S4 into an equality constraint, constructing a kinematics model of the omnidirectional four-wheel mobile mechanical arm by combining an orthogonal repetitive motion scheme of the omnidirectional four-wheel mobile mechanical arm and the anti-noise type return-to-zero neural network model to obtain an anti-noise type return-to-zero neural network controller, and then completing the track tracking and repetitive tasks of the omnidirectional four-wheel mobile mechanical arm.
2. The method for planning repetitive motion of a mobile mechanical arm of a non-convex anti-noise type return-to-zero neural network according to claim 1, wherein the specific process of step S3 is as follows:
based on the kinematics characteristics of the omnidirectional four-wheel mobile mechanical arm, a speed-level horizontal overall kinematics model of the mobile mechanical arm is constructed, and the specific mathematical expression is as follows:
in the formula, J is a coefficient matrix of the overall kinematic model;
a derivative of an actual trajectory derived for the omnidirectional mobile manipulator kinematics model with respect to time t;
is the derivative of the four wheels of the omnidirectional mobile mechanical arm and the four mechanical arm variables to the time t.
The omnidirectional four-wheel mobile mechanical arm speed level overall kinematics model, joint constraint, track tracking and a plurality of subtask conversion equations and inequality constraint time-varying quadratic programming of repeated motion of S4 are as follows:
in the formula H, q T The coefficient matrix is used for ensuring that the omnidirectional four-wheel mobile mechanical arm realizes repeated motion; upper label T A transpose operation of the representative matrix; xi + ，ξ - Representing a boundary that constrains the joint and wheel speeds within a specified range; writing inequality constraints into a matrix form design such asThe following:
K(t)ξ(t)≤d(t).
the inequality constraint time-varying quadratic programming problem is converted into an equality constraint time-varying quadratic programming problem as follows:
K(t)ξ(t)-d(t)+τ 2 (t)＝0.
in the formula tau 2 (t)∈R p ，τ 2 (t)＝τ(t)°τ(t)＝[τ 2 (t)]Is the relaxation coefficient;
constructing a time-varying quadratic programming problem of solving equality and inequality constraints by a Lagrangian function, converting the time-varying quadratic programming problem with equality and inequality constraints into a problem of solving an extremum of the Lagrangian function, and then obtaining the following equation by using a Karush-Kuhn-Tucker (KKT) condition:
Z(t)y(t)＝π(t)，
in the formula
π(t)＝[-q(t)ν(t) d(t)π(t)] T
According to a design formula of the anti-noise type return-to-zero neural network model, an error function of the system is as follows:
e(t)＝Z(t)y(t)-π(t)，
constructing a dynamic equation of the return-to-zero neural network with an interference term, wherein the mathematical expression is as follows:
firstly, the non-convex anti-noise type return-to-zero neural network is used for controlling the omnidirectional four-wheel mobile mechanical arm, so that a repetitive motion cooperative control task can be accurately completed in a time-varying noise environment, and the robustness is high. Secondly, the error change rate is restricted in a small set range by utilizing the non-convex activation function, the stability of the system is improved, the monotone increasing odd function is the special condition of the non-convex activation function, the non-convex activation function has generality, and the limitation of the monotone increasing odd function regulation of the activation function is broken through. Finally, in non-convex mapping under bounded constraints, the convex boundary constraint does not describe the reality well in robotic applications. For example, the output joint velocity or joint acceleration of the motor that moves the ith joint of the robot arm to perform the task may be discontinuous, i.e. the pulses per second used to control the motor in the robot are represented in a discrete form, which is obviously a non-convex mapping situation, and the convex boundary constraint does not meet the practical situation. The non-convex boundary constraint of the anti-noise return-to-zero neural network can accurately complete the repeated motion cooperative control of the omnidirectional four-wheel mobile mechanical arm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210632480.8A CN114800528A (en) | 2022-06-07 | 2022-06-07 | Mobile manipulator repetitive motion planning for non-convex anti-noise type return-to-zero neural network |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210632480.8A CN114800528A (en) | 2022-06-07 | 2022-06-07 | Mobile manipulator repetitive motion planning for non-convex anti-noise type return-to-zero neural network |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114800528A true CN114800528A (en) | 2022-07-29 |
Family
ID=82522204
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210632480.8A Pending CN114800528A (en) | 2022-06-07 | 2022-06-07 | Mobile manipulator repetitive motion planning for non-convex anti-noise type return-to-zero neural network |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114800528A (en) |
-
2022
- 2022-06-07 CN CN202210632480.8A patent/CN114800528A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Wang et al. | Flexible motion framework of the six wheel-legged robot: Experimental results | |
| US8738178B2 (en) | Method for controlling the walk of humanoid robot | |
| JP5506617B2 (en) | Robot control device | |
| JP5506618B2 (en) | Robot control device | |
| CN111070201B (en) | Reactive robust control method of quadruped robot based on ZMP theory under load mutation | |
| US8504208B2 (en) | Mobile object controller and floor surface estimator | |
| US8442680B2 (en) | Motion state evaluation apparatus of legged mobile robot | |
| CN113341728B (en) | Anti-noise type return-to-zero neural network four-wheel mobile mechanical arm trajectory tracking control method | |
| US8428780B2 (en) | External force target generating device of legged mobile robot | |
| Aguilera-Marinovic et al. | General dynamic model for skid-steer mobile manipulators with wheel–ground interactions | |
| Raja et al. | Learning framework for inverse kinematics of a highly redundant mobile manipulator | |
| CN111694361A (en) | Steel structure flexible flaw detection robot track tracking method based on improved approach law sliding mode control | |
| Kuo | Trajectory and heading tracking of a mecanum wheeled robot using fuzzy logic control | |
| CN111290272A (en) | Attitude stationarity adjusting method based on multi-legged robot | |
| CN113985738A (en) | Gradient neural network cooperative control of non-convex constraint omnidirectional four-wheel mobile mechanical arm repetitive motion | |
| US8442681B2 (en) | Desired motion evaluation apparatus of legged mobile robot | |
| Xu et al. | Whole-body stability control with high contact redundancy for wheel-legged hexapod robot driving over rough terrain | |
| Hyon et al. | Whole-body locomotion and posture control on a torque-controlled hydraulic rover | |
| Kameduła et al. | Wheeled motion kinematics and control of a hybrid mobility CENTAURO robot | |
| Han et al. | Robust optimal control of omni-directional mobile robot using model predictive control method | |
| US20230409035A1 (en) | Method for building controller for robot, method, device for controlling motion of robot, and robot | |
| Zheng et al. | Ball walker: A case study of humanoid robot locomotion in non-stationary environments | |
| CN114800528A (en) | Mobile manipulator repetitive motion planning for non-convex anti-noise type return-to-zero neural network | |
| Karamipour et al. | Omnidirectional mobile robot design with height and width adaptation | |
| Rocha et al. | Body attitude regulator applied to ground robots with active flippers |
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 |