CN116079743A - Modeling and anti-overturning control method for mobile mechanical arm system - Google Patents

Modeling and anti-overturning control method for mobile mechanical arm system Download PDF

Info

Publication number
CN116079743A
CN116079743A CN202310298058.8A CN202310298058A CN116079743A CN 116079743 A CN116079743 A CN 116079743A CN 202310298058 A CN202310298058 A CN 202310298058A CN 116079743 A CN116079743 A CN 116079743A
Authority
CN
China
Prior art keywords
equation
constraint
subsystem
matrix
mobile
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310298058.8A
Other languages
Chinese (zh)
Other versions
CN116079743B (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.)
Hefei University of Technology
Original Assignee
Hefei University of Technology
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 Hefei University of Technology filed Critical Hefei University of Technology
Priority to CN202310298058.8A priority Critical patent/CN116079743B/en
Publication of CN116079743A publication Critical patent/CN116079743A/en
Application granted granted Critical
Publication of CN116079743B publication Critical patent/CN116079743B/en
Active 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
    • 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
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

The invention relates to the technical field of mobile mechanical arms, in particular to a modeling and anti-overturning control method for a mobile mechanical arm system. The invention divides the complete mobile mechanical arm system into three subsystems, obtains the unconstrained kinetic equation of each subsystem, and combines the unconstrained kinetic equation into a new unconstrained equation. Combining the obtained new unconstrained kinetic equation with the constructed structural constraint equation, and utilizing the physical constraint force obtained by the U-K equation to obtain a kinetic equation of the complete system; and then, when the control is performed, calculating the performance constraint force, applying the performance constraint force to a dynamics equation, and performing coordinated control of track tracking and overturn prevention. The whole process of the invention is relatively simple, and the modeling difficulty is effectively reduced under the condition of ensuring the modeling accuracy and simplicity. When the mobile mechanical arm model is controlled, the invention introduces the anti-overturning control based on dynamics while tracking the track, and can realize coordination control.

Description

Modeling and anti-overturning control method for mobile mechanical arm system
Technical Field
The invention relates to the technical field of mobile mechanical arms, in particular to a modeling and anti-overturning control method for a mobile mechanical arm system.
Background
The mobile mechanical arm system is a complete mechanical system which is provided with one or more mechanical arms on a mobile platform and realizes coordination control, and compared with the traditional fixed base mechanical arm, the mobile mechanical arm system has a larger working range, so that the mobile mechanical arm system has a large application space in the fields of industrial production, material handling, household service, rescue and the like.
Although the mobile mechanical arm system has a powerful function, the motion forms and the motion characteristics of the two systems of the mechanical arm and the mobile platform are greatly different. This also results in a strong interaction of the two systems during operation, so that the respective motion states are interacted, i.e. the coupling effect. Moreover, this coupling effect is increasingly pronounced with an increasing mass ratio of the robot arm to the mobile platform. It is therefore common practice to weaken this coupling effect by increasing the mass of the mobile platform as much as possible, followed by the application of more scientific modeling methods.
The inventors now need to model and control a mobile robotic arm system. The mobile mechanical arm system comprises a mobile platform, a tertiary joint arranged on the mobile platform and an actuator arranged at the tail end of the tertiary joint.
There are different solutions in modeling methods of mobile robotic arm systems at present. However, these methods either require analysis of the stress conditions of each subsystem, or require solution of the inverse of the system, or require solution of the lagrangian multiplier, which can lead to complex modeling processes. Moreover, the overturn exists when the mechanical arm is moved to control, and the existing overturn prevention control mainly relates to track planning and optimization of offline movement, but the process is relatively time-consuming. The existing control mainly adopts the methods of optimal control, quadratic programming, neural network, fuzzy logic and the like, but the methods ignore dynamics, and the effect is not as expected.
Disclosure of Invention
Based on the problems, the modeling process is complex and the anti-overturning control is imperfect in the modeling method of the existing mobile mechanical arm system, and the modeling and anti-overturning control method of the mobile mechanical arm system is provided.
The invention is realized by adopting the following technical scheme:
the invention discloses a modeling and anti-overturning control method for a mobile mechanical arm system, which is used for obtaining a dynamic equation of the mobile mechanical arm system and carrying out anti-overturning constraint. The mobile mechanical arm system comprises a mobile platform, a tertiary joint arranged on the mobile platform and an actuator arranged at the tail end of the tertiary joint.
The modeling and anti-overturning control method of the mobile mechanical arm system comprises the following steps:
dividing the mobile mechanical arm system to obtain three subsystems, and performing kinematic analysis; the three subsystems comprise a first subsystem corresponding to the mobile platform, a second subsystem corresponding to the first joint and a third subsystem corresponding to the rest joints and the actuator.
Step two, respectively solving unconstrained kinetic equations of each subsystem; and discarding the constant state variable when the unconstrained kinetic equations of the second subsystem and the third subsystem are solved.
And thirdly, converting the obtained constraint into a second-order form and constructing a structural constraint equation according to the constraint on the physical structure of each subsystem in the space position.
And step four, acquiring physical constraint force of the mobile mechanical arm system by adopting a U-K method, and constructing a dynamic equation of the mobile mechanical arm system by combining all unconstrained dynamic equations and structural constraint equations.
Step five, calculating performance constraint force according to the set moving track of the moving mechanical arm system and the overturn prevention criterion acquired based on the moving mechanical arm system structure; and applying the performance constraint force to a dynamic equation of the mobile mechanical arm system, and performing coordinated control of track tracking and overturn prevention.
The mobile robotic system modeling and anti-capsizing control method implements a method or process according to embodiments of the present disclosure.
Compared with the prior art, the invention has the following beneficial effects:
1, when the mobile mechanical arm system is divided into three subsystems for modeling analysis, the stress condition of each subsystem is not required to be analyzed, the inverse solution of the system is not required to be solved, the Lagrangian multiplier is not required to be solved, and the whole process is relatively simple; the invention effectively reduces the modeling difficulty under the condition of ensuring the modeling accuracy and simplicity.
2, when the mobile mechanical arm model is controlled, dynamics-based anti-overturning control is introduced while track tracking is performed, so that coordinated control of track tracking and anti-overturning is realized, and the effect is good. In addition, the invention uses the U-K method from modeling to coordination control, so that the whole process is more modularized and flowsheet.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for the description of the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a step diagram of a modeling and anti-overturning control method for a mobile mechanical arm system according to embodiment 1 of the present invention;
FIG. 2 is a flowchart illustrating a modeling and anti-overturning control method for a mobile mechanical arm system according to embodiment 1 of the present invention;
FIG. 3 is a diagram illustrating a mobile robot system according to embodiment 1 of the present invention;
FIG. 4 is a schematic diagram of coordinates of the mobile robot system of FIG. 3;
FIG. 5 is a schematic diagram of the coordinates of the platform subsystem of FIG. 3;
FIG. 6 is a schematic diagram of the coordinates of the first joint subsystem of FIG. 3;
FIG. 7 is a schematic diagram of the coordinates of the remaining joint subsystem of FIG. 3;
FIG. 8 is a trajectory diagram of the space after the mobile platform and the tertiary joint are deployed in embodiment 2 of the present invention;
FIG. 9 is a diagram of a trajectory of motion of a tertiary joint relative to a mobile platform under constraint 1 in embodiment 2 of the present invention;
FIG. 10 is a graph showing x in example 2 of the present invention zmp A change situation diagram under constraint 1 and constraint 2;
FIG. 11 is y in example 2 of the present invention zmp A change situation diagram under constraint 1 and constraint 2;
FIG. 12 is a graph of the tracking error of the actuator trajectory under constraint 1 in example 2 of the present invention;
FIG. 13 is a graph of zmp index error under constraint 1 in example 2 of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
It is noted that when an element is referred to as being "mounted to" another element, it can be directly on the other element or intervening elements may also be present. When an element is referred to as being "disposed on" another element, it can be directly on the other element or intervening elements may also be present. When an element is referred to as being "secured to" another element, it can be directly secured to the other element or intervening elements may also be present.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "or/and" as used herein includes any and all combinations of one or more of the associated listed items.
Example 1
Referring to fig. 1, a step diagram of a modeling and anti-overturning control method for a mobile mechanical arm system according to embodiment 1 of the present invention is disclosed.
The mobile mechanical arm system applied to the method comprises a mobile platform, a tertiary joint arranged on the mobile platform and an actuator arranged at the tail end of the tertiary joint. Generally, mobile platforms have an omnidirectional orientation, categorized as having 3 degrees of freedom; the three-level joint has 3 degrees of freedom correspondingly, so that the mobile mechanical arm system has 6 degrees of freedom and can represent the existing common situation. More specifically, the mobile platform is an omnidirectional mobile platform driven by a Mecanum wheel, so that the flexibility is high, and the chassis is small. The three-stage joint simplifies the conventional six joints into the three-axis mechanical arm with only a rotary arm, a lower arm and an upper arm, and omits three other wrist joints which have no influence on the overall control precision and the working capacity of the mobile mechanical arm system, and meanwhile, the representativeness and the generality of the research are not lost.
In general, a mobile robotic arm system modeling and anti-capsizing control method includes:
step one, dividing the mobile mechanical arm system to obtain three subsystems, and performing kinematic analysis.
The mobile platform and the mechanical arm are split in consideration of the difference between the mobile platform and the mechanical arm. The mechanical arm is separated according to joint cutting, so that the complexity of the modeling process is reduced. If the segmentation is not performed, the state variable of the whole mechanical arm is increased by at least one, and the complexity of solving is increased by at least one dimension.
The three subsystems comprise a platform subsystem corresponding to the mobile platform, a second subsystem corresponding to the first joint, and a third subsystem corresponding to the rest joints and the actuator. Specifically, based on the hierarchy attribute of the U-K method, referring to fig. 2 and 3, the mobile robot system S will be described 12 Divided into three subsystems, namely a platform subsystem S 11 (i.e., the first subsystem) and two joint subsystems. The two joint subsystems comprise a first joint corresponding to the first jointJoint subsystem S 21 (i.e. the second subsystem), the remaining joint subsystem S corresponding to the remaining joints and actuators 31 (i.e., the third subsystem).
Step one, while the segmentation subsystem is being performed, a kinematic analysis is also performed. The main task of the kinematic analysis is to give the conversion relation between the joint space variable and the pose space variable, namely the problem of positive kinematics.
The specific method for the kinematic analysis comprises the following steps:
step 101, establishing a plurality of coordinate systems according to the space physical structure of the mobile mechanical arm system.
Referring to FIG. 4, the plurality of coordinate systems includes a base coordinate system { O } w { O } mobile platform coordinate system M { O } mechanical arm base coordinate system R First joint coordinate system { O } 1 Second joint coordinate system { O } 2 Third joint coordinate System { O } 3 { O, actuator coordinate System 4 }. Wherein { O w The coordinates under } are expressed as (x w 、y w 、z w )；{O M The coordinates under } are expressed as (x M 、y M 、z M ) The origin of the projection is set on the projection of the mobile platform on the ground; { O R The coordinates under } are expressed as (x R 、y R 、z R )；{O 1 The coordinates under } are expressed as (x 1 、y 1 、z 1 )；{O 2 The coordinates under } are expressed as (x 2 、y 2 、z 2 )，{O 3 The coordinates under } are expressed as (x 3 、y 3 、z 3 )；{O 4 The coordinates under } are expressed as (x 4 、y 4 、z 4 )。
The joint space variables of the mechanical arm are selected as follows: the rotation angle between the first joint and the base is theta 1 The rotation angle between the second joint and the base is theta 2 The rotation angle between the third joint and the second joint is theta 3 . Wherein, 1-3 in the footmarks respectively refer to three-stage joints of the mechanical arm, and 4 is an actuator. r is (r) i (i=1 to 4) respectively represent the distances from the center of mass of the three-stage joint and the center of mass of the actuator to the origin of the corresponding coordinate system. l (L) i (i=1 to 4) tables respectivelyThe length dimension of the tertiary joint and the actuator is shown.
And 102, calculating the motion parameters of any centroid of the mobile mechanical arm system by a D-H method.
This step is ready for subsequent kinetic analysis. Specifically, step 102 includes:
step 1021, calculating a transformation matrix of the adjacent coordinate system;
step 1022, combining the transformation matrix to obtain the motion parameters of either end in the base coordinate system.
Specifically, it can be known that using the homogeneous transformation matrix
Figure BDA0004143861710000061
(expressed from { O) i-1 Go { O } to i Transform matrix of }) can transform { O } i Arbitrary vector P in } i Conversion to { O (arbitrarily existing vector) i-1 Under }, i.e.)>
Figure BDA0004143861710000062
wherein ,P1 Representing vectors in the first joint, P 2 Representing vectors in the second joint, P 3 Representing vectors in the third joint, P 4 Representing vectors in the actuator.
Figure BDA0004143861710000063
c represents cosine cos, s represents sine sin: cθ i-1 ＝cosθ i-1 ，sθ i-1 ＝sinθ i-1 ，cα i-1 ＝cosα i-1 ，sα i-1 ＝sinα i-1 ，i＝1,2,3,4。θ i-1 Is { O i Relative to { O } i-1 Rotation angle of }. Alpha i-1 Is { O i-1 Rotation axis of { O } relative to i Rotation angle of rotation shaft. a, a i-1 Is at x i-1 { O in the direction i-2} and {Oi-1 Distance between. d, d i - 1 To at z i-1 { O in the direction i-2} and {Oi-1 Distance between.
Specifically, the mechanical arm geometric parameter table is a D-H table, as shown in Table 1:
table 1 mechanical arm geometry parameter table
Figure BDA0004143861710000064
Taking an actuator as an example:
can be obtained at { O 4 Actuator tip vector P in } 4 In { O 1 Expression P in } 0 The method comprises the following steps:
Figure BDA0004143861710000071
wherein ,
Figure BDA0004143861710000072
P 0 ＝[x y z 1] T ，P 4 ＝[0 0 0 1] T 。
in combination with table 1, the corresponding transformation relationship can be obtained:
Figure BDA0004143861710000073
/>
wherein x, y, z and 1 are vectors P 0 Is { O }, x, y, z are R X in } R 、y R 、z R The value in the direction, 1, is a meaningless value for the purpose of formatting the pair. c denotes cosine cos and s denotes sine sin.
In the kinematic analysis, a jacobian matrix is used to represent the conversion relationship between the linear velocity and angular velocity of the actuator and the velocities of the joints. Based on the above, the relationship between the end position and each joint angle can be obtained as follows:
Figure BDA0004143861710000074
the left row of the above formula indicates the linear velocity of the actuator, and the right row indicates the velocity of each joint.
wherein ,
Figure BDA0004143861710000075
reference is made to the above
Figure BDA0004143861710000076
If the speed of the actuator centroid is to be calculated +.>
Figure BDA0004143861710000077
First calculate the mass center coordinate x of the actuator c4 、y c4 、z c4 ，
Figure BDA0004143861710000078
wherein ,
Figure BDA0004143861710000081
the representation is from { O M Go { O } to W Transformation matrix, < }>
Figure BDA0004143861710000082
The representation is from { O R Go { O } to M A transform matrix;
recalculating x c4 、y c4 、z c4 Is obtained by first-order derivative of
Figure BDA0004143861710000083
For vectors of other joints, the above process is referred to, and the corresponding calculation is performed in the same way, or the corresponding calculation can be obtained
Figure BDA0004143861710000084
(the speed of the first joint centroid in three directions); />
Figure BDA0004143861710000085
(the speed of the second joint centroid in three directions); />
Figure BDA0004143861710000086
(the speed of the third joint centroid in three directions); namely:
Figure BDA0004143861710000087
the results obtained are used in the subsequent construction of unconstrained kinetic equations for the two joint subsystems.
And step two, respectively solving unconstrained kinetic equations of each subsystem to obtain unconstrained kinetic equations with the same number as the subsystems.
Since three subsystems are already segmented in the first step, the unconstrained dynamics equations of the subsystems are respectively required to be extracted in the second step.
The mobile robot arm system S has been mentioned above 12 Divided into three subsystems, namely a platform subsystem S 11 First joint subsystem S 21 And the rest of the joint subsystem S 31 . Wherein for the first joint subsystem S 21 And the rest of the joint subsystem S 31 When the unconstrained kinetic equation is solved, the constant state variable is abandoned.
The method is characterized in that each subsystem moves in a three-dimensional space, when an unconstrained dynamics model is established by using a Lagrange method, the general thought can jump out of the original motion state scene of each subsystem in the whole system, and all state variables representing the motion state of the subsystem can be included in Lagrange equation calculation to obtain the unconstrained dynamics equation of the subsystem. In practice, some state variables are constants in the motion scene of the whole system, and the kinetic energy term related to the state variable is zero, so that the constant state variables are abandoned in the process of selecting the state variables, and the processing process is simpler, the state variables of the subsystem are minimum, the form is simplest and the calculation burden is smaller when the unconstrained dynamic model of the subsystem is constructed.
(one) for platform subsystem S 11 Establishing a motion equation of the mobile platform under the base standard system; extracting a relation matrix of a motion equation of the mobile platform, and performing speed transformation on the relation matrix; obtaining a general dynamic equation of the mobile platform according to the Lagrangian equation; and converting a general dynamic equation of the mobile platform by combining the matrix obtained by the speed transformation to obtain a dynamic equation under generalized coordinates, and carrying out simplified calculation to obtain an unconstrained dynamic equation of the platform subsystem.
Specifically, referring to FIG. 5, S 11 At { O w }、{O M In }. To characterize the position of the mobile platform, generalized coordinates x, y are used,
Figure BDA00041438617100000912
The position information is described, and the first derivative is the moving speed and the second derivative is the moving acceleration. Because the mobile platform is driven omnidirectionally, each wheel also corresponds to an angular displacement, respectively theta ω1 、θ ω2 、θ ω3 、θ ω4 In addition, the first derivative is angular velocity and the second derivative is angular acceleration. Meanwhile, as can be seen from FIG. 5, the length of the movable platform is 2L, the width is 2L, and the radius of the wheels is R ω 。
The motion equation of the mobile platform is expressed as:
Figure BDA0004143861710000091
wherein ,
Figure BDA0004143861710000092
representing the mobile platform x, y,/->
Figure BDA0004143861710000093
I.e. the speed of movement in three directions.
Figure BDA0004143861710000094
Representing angular displacement theta ω1 、θ ω2 、θ ω3 、θ ω4 I.e. angular velocity).
I.e. transform matrix
Figure BDA0004143861710000095
Similarly, from
Figure BDA0004143861710000096
To->
Figure BDA0004143861710000097
Transform matrix B of 2 ：
Figure BDA0004143861710000098
Since the platform subsystem is set as S 11 The kinetic equation is readily available from the lagrangian equation:
Figure BDA0004143861710000099
/>
wherein ,
Figure BDA00041438617100000910
representing the angular velocity matrix of the wheel>
Figure BDA00041438617100000911
Representing a matrix of wheel accelerations and,
M v ∈R 4×4 representing an inertial matrix, D v ∈R 4×4 Indicating friction-induced forces, u 1 Representing a driving moment matrix.
In particular, the method comprises the steps of,
Figure BDA0004143861710000101
θ ω ＝[θ ω1 θ ω2 θ ω3 θ ω4 ] T ；D v ＝λ·I 3 ；u 1 ＝[u 11 u 12 u 13 u 14 ] T ；
wherein ,u11 、u 12 、u 13 、u 14 Driving moment of four driving shafts for the moving platform. m represents the mass of the mobile platform. J (J) w Representing the moment of inertia of each wheel of the mobile platform about its own axis. J (J) Z Representing the moment of inertia of the mobile platform about the Z axis. Lambda represents the coefficient of friction. I 3 Representing a 3-order identity matrix.
Converting the obtained kinetic equation into a kinetic equation under generalized coordinates, and expressing the kinetic equation as:
Figure BDA0004143861710000102
wherein ,q11 Representation platform subsystem S 11 Is used to determine the state variable of (1),
Figure BDA0004143861710000103
Figure BDA0004143861710000104
represents q 11 Is used for the first order of the leads,
Figure BDA0004143861710000105
represents q 11 Is a second derivative of (c).
Simplifying the above to S 11 ：
Figure BDA0004143861710000106
wherein ,M11 Representation platform subsystem S 11 M matrix of>
Figure BDA0004143861710000107
Representation platform subsystem S 11 State variable, Q 11 Representation platform subsystem S 11 Q matrix of (a);
Figure BDA0004143861710000108
(II) for the first joint subsystem S 21 And determining coordinate parameters of the first joint subsystem under a base coordinate system, establishing a Lagrange function of the first joint subsystem under the base coordinate system, and obtaining an unconstrained kinetic equation of the first joint subsystem according to the Lagrange equation.
Specifically, referring to FIG. 6, S 21 At { O w }、{O 1 In }.
Selecting generalized coordinate q 21 ＝[x 01 y 01 θ 01 ]The state variable is x 01 、y 01 、θ 01 . This is because of (x) 01 ,y 01 ,z 01 ) Is { O 1 The { O } coordinates are w Spatial location of the } coordinate, where z 01 Is constant (H) and not as a state variable. (alpha) 01 ,β 01 ,θ 01 ) Is { O 1 Euler angles, chosen as XYZ representation, where α 01 ＝0、β 01 =0 and not as a state variable,
Figure BDA0004143861710000111
S 21 lagrangian function L of (2) 21 The method comprises the following steps:
Figure BDA0004143861710000112
wherein ,m1 Is the mass of the first joint. I 1 Is the moment of inertia of the first joint.
Figure BDA0004143861710000113
Is the velocity of the first joint centroid in three directions. />
Figure BDA0004143861710000114
Is the rotational speed of the first joint. H+r 1 The first joint centroid is the base punctuation height.
X is the number c1 、y c1 、z c1 And state variable x 01 、y 01 、θ 01 There is a conversion relationship, which is not developed here.
From the Lagrangian equation:
Figure BDA0004143861710000115
and then obtain S 21 The expression of the kinetic equation under the unconstrained condition is as follows:
Figure BDA0004143861710000116
wherein ,M21 Representing a first joint subsystem S 21 M matrix, q of 21 Representing a first joint subsystem S 21 Is used to determine the state variable of (1),
Figure BDA0004143861710000117
represents q 21 Is (are) first order guide,/->
Figure BDA0004143861710000118
Represents q 21 Is of the second derivative, Q 21 Representing a first joint subsystem S 21 Is a Q matrix of (c).
(III) for the remaining Joint subsystems S 31 And determining coordinate parameters of the rest joint subsystems under a base coordinate system, establishing Lagrange functions of the rest joint subsystems under the base coordinate system, and obtaining unconstrained dynamics equations of the rest joint subsystems according to the Lagrange equations.
Specifically, referring to FIG. 7, S 31 At { O w }、{O 2 }、{O 3 }、{O 4 In }.
Selecting generalized coordinate q 31 ＝[x 02 y 02 γ θ 2 θ 3 ]The state variable is x 02 、y 02 、γ、θ 2 、θ 3 . This is because of (x) 02 ,y 02 ,z 02 ) Is { O 2 The { O } coordinates are w Spatial position of }, z 02 Is constant (H+l) 1 ) And not as state variables. (gamma, phi, theta) 2 ) Is { O 2 Euler angles, chosen as ZXZ, represent methods. Wherein ψ=90° is not a state variable due to being a constant value;
Figure BDA0004143861710000121
S 31 lagrangian function L of (2) 31 The method comprises the following steps:
Figure BDA0004143861710000122
wherein ,m2 、m 3 、m 4 The mass of the second joint, the third joint and the actuator respectively. I 2 、I 3 、I 4 The moment of inertia of the second joint, the third joint and the actuator respectively.
Figure BDA0004143861710000123
For theta 2 、θ 3 Is a first order derivative of (a). />
Figure BDA0004143861710000124
Is the first derivative of gamma.
P is gravitational potential energy, specifically:
Figure BDA0004143861710000125
x is the number c2 、y c2 、z c2 、x c3 、y c3 、z c3 、x c4 、y c4 、z c4 And state variable x 02 、y 02 、γ、θ 2 、θ 3 There is a conversion relationship, which is not developed here.
From the Lagrangian equation:
Figure BDA0004143861710000126
/>
and then obtain S 31 The expression of the kinetic equation under the unconstrained condition is as follows:
Figure BDA0004143861710000127
wherein ,M31 Representing the remaining joint subsystem S 31 M matrix, q of 31 Representing the remaining joint subsystem S 31 Is used to determine the state variable of (1),
Figure BDA0004143861710000128
represents q 31 Is (are) first order guide,/->
Figure BDA0004143861710000129
Represents q 31 Is of the second derivative, Q 31 Representing the remaining joint subsystem S 31 Is a Q matrix of (c).
For x ci 、y ci 、z ci The relationship between (i=1 to 4) and the state variables is as follows:
x c1 ＝x 02 +r 2 cθ 2 cγ；y c1 ＝y 01 +r 2 cθ 2 sγ；z c1 ＝z 01 +r 1 ；
x c2 ＝x 02 +r 2 cθ 2 cγ；y c2 ＝y 01 +r 2 cθ 2 sγ；z c2 ＝z 02 +r 2 sθ 2 ；
x c3 ＝x 02 +l 2 cθ 2 cγ+r 3 c(θ 2 -θ 3 )cγ；
y c3 ＝y 01 +l 2 cθ 2 sγ；
z c3 ＝z c2 +r 2 sθ 2 +r 3 s(θ 2 -θ 3 )；
x c4 ＝x 02 +l 2 cθ 2 cγ+(l 3 +r 4 )c(θ 2 -θ 3 )cγ；
y c4 ＝y 01 +l 2 cθ 2 sγ+(l 3 +r 4 )c(θ 2 -θ 3 )sγ；
z c4 ＝z c2 +r 2 sθ 2 +(l 3 +r 4 )s(θ 2 -θ 3 )。
and thirdly, converting the obtained constraint into a second-order form and constructing a structural constraint equation according to the constraint on the physical structure of each subsystem in the space position.
The third step aims at constructing a structural constraint equation for constraining each subsystem. Specifically, a constraint relation is determined according to physical structure constraint of the mobile mechanical arm system, and then the constraint relation is converted into a structure constraint equation in a matrix form.
The mobile mechanical arm system can pass through the stacking subsystem S 11 、S 21 、S 31 And then is complemented by physical structural constraint to reconstruct the system into an organic complete system. Based on the spatial positional relationship, the approximate relationship can be obtained as follows: x=x 01 ＝x 02 ，y＝y 01 ＝y 02 ，γ＝θ 01 Further obtain the structural constraint equation as
Figure BDA0004143861710000134
wherein ,
Figure BDA0004143861710000131
Figure BDA0004143861710000132
is q 12 Is a second derivative of (2); q 12 ＝[x y z x 01 y 01 θ 01 x 02 y 02 γ θ 2 θ 3 ] T Is the total state variable of the mobile mechanical arm system.
Thus, the structural constraint equation can be written as:
Figure BDA0004143861710000133
wherein ,05×1 Zero vector matrix of 5×1, 0 5×1 ＝[0 0 0 0 0] T 。
And step four, acquiring physical constraint force of the mobile mechanical arm system by adopting a U-K method, and constructing a dynamic equation of the mobile mechanical arm system by combining all unconstrained dynamic equations and structural constraint equations.
And step four, the kinetic equation of the whole mobile mechanical arm system is required to be obtained, and modeling is completed.
In general, step four comprises: extracting an M matrix (i.e. a mass matrix) and a Q matrix (i.e. a matrix containing centrifugal force/coriolis force, gravity and external force) from the obtained unconstrained kinetic equation; synthesizing all M matrixes into a new M matrix, and synthesizing all Q matrixes into a new Q matrix; acquiring physical constraint force of the mobile mechanical arm system by utilizing a U-K equation; based on the new M matrix, the new Q matrix, the structural constraint equation and the physical constraint force, a complete constrained dynamics equation is constructed, and a dynamics equation of the mobile mechanical arm system is obtained in a simplified manner.
More specifically, the fourth step includes:
first from S 11 Extraction of M from unconstrained kinetic equation of (2) 11 、Q 11 The method comprises the steps of carrying out a first treatment on the surface of the From S 21 Extraction of M from unconstrained kinetic equation of (2) 21 、Q 21 The method comprises the steps of carrying out a first treatment on the surface of the From S 31 Extraction of M from unconstrained kinetic equation of (2) 31 、Q 31 。
From M 11 、M 21 、M 31 Concentrated writing into a new matrix M 12 From Q 11 、Q 21 、Q 31 Concentrated writing into a new matrix Q 12 ；
wherein ,
Figure BDA0004143861710000141
Q 12 ＝[Q 11 Q 21 Q 31 ] T 。
obtaining physical constraint force of mobile mechanical arm system by using U-K equation
Figure BDA0004143861710000142
Figure BDA0004143861710000143
wherein ,
Figure BDA0004143861710000144
representing the physical constraint force applied by the physical constraint; />
Figure BDA0004143861710000145
Represents M 12 To the 1/2 th power of (2)>
Figure BDA0004143861710000146
Represents M 12 To the-1/2 th power of (2),>
Figure BDA0004143861710000147
represents M 12 To the power of-1 (S) + Indicating that a molar Penrose inversion was performed.
Then, based on
Figure BDA0004143861710000148
M 12 、Q 12、 and />
Figure BDA0004143861710000149
Constructing a complete constrained dynamics equation: s is S 12 ：
Figure BDA00041438617100001410
Figure BDA00041438617100001411
Is q 12 Is a first order derivative of (a). Since the above formula is not the simplest form, it is necessary to reduce it, so q is found 12 With state variables truly required by the system, i.e. just-needed state variables
Figure BDA00041438617100001412
A transformation relationship between the two.
In addition to constraint relationships, there are
Figure BDA0004143861710000151
Therefore, by combining these conditions, the following relation can be obtained:
Figure BDA0004143861710000152
wherein ,
Figure BDA0004143861710000153
/>
Figure BDA0004143861710000154
Figure BDA0004143861710000155
is q 0 Is a second derivative of (2); q 0 To characterize the state variables of the mobile robotic arm system,
Figure BDA0004143861710000156
x, y, and->
Figure BDA0004143861710000157
θ 1 、θ 2 、θ 3 Is a second derivative of (c).
Thereby S is arranged as 12 ：
Figure BDA0004143861710000158
The conversion is as follows:
Figure BDA0004143861710000159
and further simplified into: s is S 12 ：
Figure BDA00041438617100001510
wherein ,M0 M matrix, q representing a mobile robotic arm system 0 Representing the as-needed state variables of the mobile robotic arm system,
Figure BDA00041438617100001511
is q 0 First order derivative of->
Figure BDA00041438617100001512
Is q 0 Is of the second derivative, Q 0 Q matrix representing a mobile robotic arm system, +.>
Figure BDA00041438617100001513
Is the physical restraining force after simplification.
Step five, calculating performance constraint force according to the set moving track of the moving mechanical arm system and the overturn prevention criterion acquired based on the moving mechanical arm system structure; and applying the performance constraint force to a dynamic equation of the mobile mechanical arm system, and performing coordinated control of track tracking and overturn prevention.
And step four, modeling of the mobile mechanical arm system is completed, and anti-overturning control is performed on the mobile mechanical arm system model.
The method for calculating the performance constraint force comprises the following steps:
step A, correcting the set moving track to obtain a corrected track constraint equation:
since the system initial conditions used for model simulation must meet the required trajectory tracking constraints, this is unlikely to happen due to the presence of various factors, which may only be met approximately at the initial time. If the initial conditions are not compatible, the simulation results diverge. Therefore, the initial condition deviation needs to be corrected through the step A, so that the accuracy of track tracking is ensured.
Step A is based on Lyapunov stability theory, and comprises the following specific operations:
based on a track stabilization method, an original constraint equation is calculated
Figure BDA0004143861710000161
The modification is as the correction formula:
Figure BDA0004143861710000162
when kappa is i ,ε i The equilibrium point is asymptotically stable, i.e. > 0
Figure BDA0004143861710000163
And taking the correction formula as a desired track requirement, and obtaining a correction track constraint equation through a differentiation process. The modified trajectory constraint equation is expressed in the form of a matrix equation:
Figure BDA0004143861710000164
A 1 、b 1 to correct the coefficient matrix of the trajectory constraint, t characterizes the time.
Step B, converting the anti-overturning criterion into an anti-overturning constraint equation:
the anti-capsizing criterion selected in this example 2 uses zero moment points (Zero Moment Point, ZMP). ZMP is defined as the point on the ground where the sum of all active moments is zero. If ZMP is located within a support polygon (for a mobile robot arm model, the support polygon is referred to as a mobile platform), the system is stable.
The specific operation of the step B is as follows:
selecting an anti-overturning criterion, wherein the mathematical expression form is as follows:
Figure BDA0004143861710000165
Figure BDA0004143861710000166
Figure BDA0004143861710000167
in the formula ,mi For the mass of the ith object, x i ,y i ,z i For object { O M A position in the x-ray tube,
Figure BDA0004143861710000168
for object { O M Acceleration in }, T i Moment generated for the angular velocity and angular acceleration of the object; (T) x ) i Is T i Component in x direction, (T) y ) i Is T i A component in the y direction; i i For moment of inertia of the object +.>
Figure BDA0004143861710000169
For angular acceleration of the object, ω i G is the angular velocity of the object and g is the gravitational acceleration.
Then to x zmp 、y zmp And (3) constraining, namely converting the constraint into a constraint condition which can be used by the U-K method. Due to x zmp 、y zmp The expression of (c) contains variables of zero, first and second order, so that mathematical transformation thereof can be converted into the desired constraint form, i.e., into constraint equations of second order.
For x zmp Calculate its expected value
Figure BDA0004143861710000171
In the above, F 1i (i＝1～4)、F 2j (j=1 to 2) are coefficients (including expressions on the first and zero orders of the state variables) after combining the homomorphism terms for the numerator and denominator parts, respectively, F 15 、F 23 The remainder not containing the quadratic term, respectively;
Figure BDA0004143861710000172
for acceleration of the mobile platform in x-direction, +.>
Figure BDA0004143861710000173
For the angular acceleration of the first joint +.>
Figure BDA0004143861710000174
For the angular acceleration of the second joint +.>
Figure BDA0004143861710000175
Is the angular acceleration of the third joint.
And (3) formalizing the fractional form to obtain:
Figure BDA0004143861710000176
similarly, for y zmp The same treatment was performed to obtain:
Figure BDA0004143861710000177
in the above, F 3i (i＝1～4)、F 2j (j=1-2) is the coefficient after combining the homomorphism term for the numerator and denominator parts, F 35 、F 23 The remainder not containing the quadratic term, respectively;
Figure BDA0004143861710000178
for acceleration of the mobile platform in y-direction, +.>
Figure BDA0004143861710000179
Is y zmp Is a desired value of (2).
In this way, two performance constraints on anti-capsizing are obtained and converted into a matrix form:
Figure BDA00041438617100001710
A 2 、b 2 and t represents time for the coefficient matrix of the anti-overturning constraint.
It should be noted that, step A, B does not emphasize the order, and both steps may be performed simultaneously.
And C, combining the corrected track constraint equation and the anti-overturning constraint equation into an integral performance constraint matrix, and solving based on the U-K equation to obtain the performance constraint force.
Combining the corrected track constraint equation of the step A and the anti-overturning constraint equation of the step B to form an integral performance constraint matrix:
Figure BDA00041438617100001711
A 3 、b 3 and t represents time for a coefficient matrix of performance constraint.
And calculating to obtain performance constraint force based on a U-K equation:
Figure BDA0004143861710000181
wherein ,
Figure BDA0004143861710000182
representing performance constraint, ->
Figure BDA0004143861710000183
Represents M 0 To the 1/2 th power of (2)>
Figure BDA0004143861710000184
Represents M 0 To the-1/2 th power of (2),>
Figure BDA0004143861710000185
represents M 0 To the-1 power of (2).
Then, a performance constraint force is applied to a dynamic equation of the mobile mechanical arm system, and a dynamic control model is constructed. The dynamics control model is expressed as
Figure BDA0004143861710000186
The method is used for moving the mechanical arm model to track and prevent overturning. The dynamics control model is loaded subsequentlyAnd carrying out coordination control of track tracking and overturn prevention on the mobile mechanical arm system model.
The embodiment 1 synchronously discloses a modeling and anti-overturning control device for a mobile mechanical arm system, and the modeling and anti-overturning control method for the mobile mechanical arm system is used.
The mobile mechanical arm system modeling and anti-overturning control device comprises a subsystem construction module, an unconstrained kinetic equation construction module, a structural constraint equation construction module, a system kinetic equation construction module and a model control module.
The subsystem construction module is used for dividing the mobile mechanical arm system to obtain three subsystems and performing kinematic analysis. The unconstrained kinetic equation construction module is used for solving the unconstrained kinetic equation of each subsystem. The structural constraint equation construction module is used for converting the obtained constraint into a second-order form and constructing a structural constraint equation according to the constraint on the physical structure of each subsystem in the space position. The system dynamics equation construction module is used for acquiring the physical constraint force of the mobile mechanical arm system by adopting a U-K method, and constructing the dynamics equation of the mobile mechanical arm system by combining all unconstrained dynamics equations and structural constraint equations. The model control module is used for calculating performance constraint capacity according to the set moving track of the mobile mechanical arm system and the overturn prevention criterion acquired based on the structure of the mobile mechanical arm system; and applying the performance constraint force to a dynamic equation of the mobile mechanical arm system, and performing coordinated control of track tracking and overturn prevention.
The present embodiment 1 also provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by the processor, the mobile robot system modeling and anti-overturning control method in embodiment 1 is realized. The method of embodiment 1 may be applied in the form of software, such as a program designed to be independently executable on a computer-readable storage medium, which may be a usb disk, designed as a U-shield, through which the program of the entire method is designed to be started by external triggering.
Example 2
This example 2 was simulated for example 1 to verify the effectiveness of the modeling proposed in example 1. And meanwhile, the feasibility and the adaptability of track tracking and anti-overturning maintenance are verified and illustrated through a simulation process.
The simulation object is a composite system composed of a mobile platform and three-stage joints adopted in the embodiment 1, and the mechanical device mainly comprises an omnidirectional mobile platform driven by 4 Mecanum wheels and a mechanical arm part composed of three-stage joints and end clamping jaws (actuators), wherein specific parameters are shown in the table 2.
TABLE 1 System specific parameter Table
Figure BDA0004143861710000191
In order to make the whole control process clearly appear, setting relative track constraint relative to the mobile platform for the mechanical arm part so that the mechanical arm part performs oblique circular motion relative to the mobile platform. Two controls were set, incorporating the anti-capsizing constraint into constraint equation (constraint 1) and without the anti-capsizing constraint (constraint 2), respectively. Specifically, the results are shown in Table 3.
TABLE 3 constraint parameter Table
Figure BDA0004143861710000192
The difference between constraint 1 and constraint 2 is: whether or not to apply anti-capsizing constraint F 4 、F 5 。
Wherein F selected in constraint 1 1 、F 2 、F 3 All are complete constraints and can be rewritten into a correction form. Wherein the parameters are set as follows: kappa (kappa) m =2.5 (m=1 to 3, corresponds to F 1 ～F 3 Modified version of (2) epsilon n =2 (n=1 to 3, corresponds to F) 1 ～F 3 Is a modified version of (c).
And simulating the constraint 1 and the constraint 2 respectively to obtain the change condition of the track and zmp index under the motion condition.
Referring to fig. 8 and 9, a trajectory diagram of the mobile manipulator model under constraint 1 is shown. Fig. 8 is a track of a space after the moving platform and the mechanical arm are partially unfolded, which accords with a common coordinated movement mode of the moving mechanical arm. FIG. 9 is a drawing of a circle of the arm portion relative to the mobile platform under constraint 1, which may be found on a macroscopic off-line trajectory of the actuator from gradual regression over a wide range of initial condition deviations. The model constructed by the method is correct and feasible.
Referring to fig. 10 and 11, fig. 10 reflects x zmp The change under constraint 1, constraint 2, FIG. 11 reflects y zmp Change cases under constraint 1, constraint 2. It is known that under constraint 2, no anti-toppling constraint is imposed, x zmp 、y zmp The limit value of + -0.25 is exceeded (+ -0.25 is obtained depending on the structural size of the support frame of the mobile platform), so that the anti-capsizing requirement is not satisfied. Under constraint 1, however, the anti-toppling requirement can be easily met without large fluctuations due to the application of the anti-toppling constraint.
Referring to fig. 12 and 13, fig. 12 reflects the tracking error change condition of the actuator track under constraint 1, and fig. 13 reflects the zmp index error change condition of the whole model under constraint 1. It can be seen that the actuator gradually converges after 10s and completely converges after 15s, which indicates that the trajectory tracking meets the precision requirement. X is x zmp 、y zmp Maintain the error at 10 -4 m magnitude, also meets the precision requirement. The control method can realize the double requirements of track tracking and anti-overturning control.
The technical features of the above-described embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above-described embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.
The above examples illustrate only a few embodiments of the invention, which are described in detail and are not to be construed as limiting the scope of the invention. It should be noted that it will be apparent to those skilled in the art that several variations and modifications can be made without departing from the spirit of the invention, which are all within the scope of the invention. Accordingly, the scope of protection of the present invention is to be determined by the appended claims.

Claims (10)

1. The modeling and anti-overturning control method for the mobile mechanical arm system is used for obtaining a dynamic equation of the mobile mechanical arm system and carrying out anti-overturning constraint; the mobile mechanical arm system comprises a mobile platform, a tertiary joint arranged on the mobile platform and an actuator arranged at the tail end of the tertiary joint; it is characterized in that the method comprises the steps of,
the modeling and anti-overturning control method of the mobile mechanical arm system comprises the following steps:
dividing the mobile mechanical arm system to obtain three subsystems, and performing kinematic analysis; the three subsystems comprise a first subsystem corresponding to the mobile platform, a second subsystem corresponding to the first joint and a third subsystem corresponding to the rest joints and the actuator;
step two, respectively solving unconstrained kinetic equations of each subsystem; when the unconstrained kinetic equations of the second subsystem and the third subsystem are solved, discarding the constant state variables;
thirdly, converting the obtained constraint into a second-order form and constructing a structural constraint equation according to the constraint on the physical structure of each subsystem in the space position;
step four, acquiring physical constraint force of the mobile mechanical arm system by adopting a U-K method, and constructing a dynamic equation of the mobile mechanical arm system by combining all unconstrained dynamic equations and structural constraint equations;
step five, calculating performance constraint force according to the set moving track of the moving mechanical arm system and the overturn prevention criterion acquired based on the moving mechanical arm system structure; and applying the performance constraint force to a dynamic equation of the mobile mechanical arm system, and performing coordinated control of track tracking and overturn prevention.
2. The method for modeling and anti-capsizing control of a mobile robot system according to claim 1, wherein in the first step, the method for performing a kinematic analysis comprises:
step 101, establishing a plurality of coordinate systems according to a space physical structure of a mobile mechanical arm system;
wherein the plurality of coordinate systems comprises a base coordinate system { O } w { O } mobile platform coordinate system M { O } mechanical arm base coordinate system R First joint coordinate system { O } 1 Second joint coordinate system { O } 2 Third joint coordinate System { O } 3 { O, actuator coordinate System 4 }；
And 102, calculating the motion parameters of any centroid of the mobile mechanical arm system by a D-H method.
3. The mobile robotic arm system modeling and anti-capsizing control method of claim 2, wherein step 102 comprises:
step 1021, calculating transformation matrix of adjacent coordinate system
Figure FDA0004143861690000021
wherein ,
Figure FDA0004143861690000022
the representation is from { O M Go { O } to W Transformation matrix, < }>
Figure FDA0004143861690000023
The representation is from { O R Go { O } to M A transform matrix; />
Figure FDA0004143861690000024
The representation is from { O i-1 Go { O } to i A transform matrix;
step 1022, combining the transformation matrix to obtain the motion parameters of any centroid of the mobile mechanical arm system in the base coordinate system;
wherein, the motion parameters of any centroid of the mobile mechanical arm system in the basic coordinate system are expressed as
Figure FDA0004143861690000025
x c1 、y c1 、z c1 Represents the first joint centroid, r 1 To { O for the first joint centroid 1 Distance of origin;
x c2 、y c2 、z c2 represents the second joint centroid, r 2 To { O for the second joint centroid 2 Distance of origin;
x c3 、y c3 、z c3 represents the third joint centroid, r 3 To { O } of the third joint centroid 3 Distance of origin;
x c4 、y c4 、z c4 representing the actuator centroid, r 4 To { O for actuator centroid 4 Distance of origin.
4. The modeling and anti-overturning control method of a mobile mechanical arm system according to claim 3, wherein in the second step, the method for solving the unconstrained kinetic equation of the first subsystem comprises:
establishing a motion equation of the mobile platform under a base coordinate system; the motion equation of the mobile platform is as follows:
Figure FDA0004143861690000026
wherein ,
Figure FDA0004143861690000027
representing the speed of movement of the mobile platform in three directions, < >>
Figure FDA0004143861690000028
Angular velocities of four wheels representing the mobile platform; l represents half the length of the mobile platform, L represents half the width of the mobile platform, R ω Radius of four wheels representing the mobile platform;
extracting a transformation matrix B of the speed transformation of the mobile platform 1 、B 2 ；
wherein ,
Figure FDA0004143861690000031
Figure FDA0004143861690000032
obtaining a dynamic equation of the mobile platform according to the Lagrangian equation; the dynamic equation of the mobile platform is as follows:
Figure FDA0004143861690000033
wherein ,Mv Representing an inertial matrix, D v Indicating the force caused by the friction and,
Figure FDA0004143861690000034
representing a wheel angular velocity matrix; />
Figure FDA0004143861690000035
Representing a wheel acceleration matrix, u 1 Representing a driving moment matrix;
converting the dynamic equation of the mobile platform by combining the transformation matrix to obtain a dynamic equation under generalized coordinates and simplifying the dynamic equation to obtain an unconstrained dynamic equation of the first subsystem;
the dynamic equation under the generalized coordinates of the first subsystem is as follows:
Figure FDA0004143861690000036
q 11 a state variable representing the first subsystem, +.>
Figure FDA0004143861690000037
Represents q 11 Is (are) first order guide,/->
Figure FDA0004143861690000038
Represents q 11 Is a second derivative of (2);
the unconstrained kinetic equation of the first subsystem is:
Figure FDA0004143861690000039
M 11 representing M matrix, Q of first subsystem 11 Representing the Q matrix of the first subsystem.
5. The method for modeling and anti-overturning control of a mobile mechanical arm system according to claim 4, wherein in the second step, the method for solving the unconstrained kinetic equation of the second subsystem comprises:
determining the coordinate parameter of the second subsystem under the basic coordinate system as x 01 、y 01 、θ 01 ；
Establishing a Lagrangian function of the second subsystem under the base standard system;
obtaining an unconstrained kinetic equation of the second subsystem according to the Lagrangian equation; the unconstrained kinetic equation of the second subsystem is that
Figure FDA00041438616900000310
wherein ,M21 Representing the M matrix, q of the second subsystem 21 State variables representing the second subsystem, +.>
Figure FDA00041438616900000311
Represents q 21 Is (are) first order guide,/->
Figure FDA00041438616900000312
Represents q 21 Is of the second derivative, Q 21 Representing the Q matrix of the second subsystem.
6. The method for modeling and anti-overturning control of a mobile mechanical arm system according to claim 5, wherein in the second step, the method for solving the unconstrained kinetic equation of the third subsystem comprises:
determination of the third subsystemThe coordinate parameter under the basic coordinate system is x 02 、y 02 、γ、θ 2 、θ 3 ；
Establishing a Lagrangian function of a third subsystem under the base standard system;
obtaining an unconstrained kinetic equation of the third subsystem according to the Lagrangian equation; the unconstrained kinetic equation of the third subsystem is that
Figure FDA0004143861690000041
wherein ,M31 Representing M matrix, q of the third subsystem 31 State variables representing the third subsystem, +.>
Figure FDA0004143861690000042
Represents q 31 Is (are) first order guide,/->
Figure FDA0004143861690000043
Represents q 31 Is of the second derivative, Q 31 Representing the Q matrix of the third subsystem.
7. The mobile robot system modeling and anti-capsizing control method according to claim 6, wherein the third step comprises:
determining a constraint relation according to the physical structure constraint of the mobile mechanical arm; the constraint relation is as follows: x=x 01 ＝x 02 ，y＝y 01 ＝y 02 ，γ＝θ 01 ；
Converting the constraint relation into a structural constraint equation in a matrix form; the structural constraint equation is:
Figure FDA0004143861690000044
wherein ,/>
Figure FDA0004143861690000045
q 12 Representing all state variables of the mobile robotic arm system.
8. The mobile robot system modeling and anti-capsizing control method according to claim 7, wherein the fourth step comprises:
extracting an M matrix and a Q matrix from all unconstrained dynamic equations;
synthesizing all M matrixes into a new M matrix, and synthesizing all Q matrixes into a new Q matrix; wherein the new M matrix is
Figure FDA0004143861690000046
The new Q matrix is Q 12 ＝[Q 11 Q 21 Q 31 ] T ；
Acquiring physical constraint force of the mobile mechanical arm system by utilizing a U-K equation; the physical constraint force is
Figure FDA0004143861690000051
wherein ,/>
Figure FDA0004143861690000052
Representing the physical constraint force applied by the physical constraint; />
Figure FDA0004143861690000053
Represents M 12 To the 1/2 th power of (2)>
Figure FDA0004143861690000054
Represents M 12 To the-1/2 th power of (2),>
Figure FDA0004143861690000055
represents M 12 To the power of-1 (S) + Represents the molar Penrose inversion;
based on the new M matrix, the new Q matrix, the structural constraint equation and the physical constraint force, constructing a complete constrained dynamics equation, and simplifying to obtain a dynamics equation of the mobile mechanical arm system;
the kinetic equation of the mobile mechanical arm system is as follows:
Figure FDA0004143861690000056
wherein ,M0 M matrix, q representing a mobile robotic arm system 0 Representing the just needed state variable of the mobile manipulator system, < ->
Figure FDA0004143861690000057
Is q 0 First order derivative of->
Figure FDA0004143861690000058
Is q 0 Is of the second derivative, Q 0 Q matrix representing a mobile robotic arm system, +.>
Figure FDA0004143861690000059
Is the physical restraining force after simplification.
9. The method for modeling and anti-overturning control of a mobile robot system according to claim 8, wherein in the fifth step, the method for calculating the performance constraint force comprises:
correcting the set moving track to obtain a corrected track constraint equation; the modified track constraint equation is
Figure FDA00041438616900000510
wherein ,A1 、b 1 T represents time for correcting the coefficient matrix of the track constraint;
converting the anti-overturning criterion into an anti-overturning constraint equation; the anti-overturning constraint equation is that
Figure FDA00041438616900000511
wherein ,A2 、b 2 Coefficient matrix for anti-capsizing constraint;
combining the correction track constraint equation and the anti-overturning constraint equation into an integral performance constraint matrix, and solving based on a U-K equation to obtain performance constraint force;
the performance constraintsThe matrix is
Figure FDA00041438616900000512
The performance constraint force is
Figure FDA00041438616900000513
wherein ,A3 、b 3 A coefficient matrix that is a performance constraint;
Figure FDA00041438616900000514
representing performance constraint, ->
Figure FDA00041438616900000515
Represents M 0 To the 1/2 th power of (2)>
Figure FDA00041438616900000516
Represents M 0 To the-1/2 th power of (2),>
Figure FDA00041438616900000517
represents M 0 To the-1 power of (2).
10. A computer readable storage medium having stored thereon a computer program, wherein the computer program, when executed by a processor, implements the mobile robot system modeling and anti-capsizing control method according to any of claims 1-9.
CN202310298058.8A 2023-03-24 2023-03-24 Modeling and anti-overturning control method for mobile mechanical arm system Active CN116079743B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310298058.8A CN116079743B (en) 2023-03-24 2023-03-24 Modeling and anti-overturning control method for mobile mechanical arm system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310298058.8A CN116079743B (en) 2023-03-24 2023-03-24 Modeling and anti-overturning control method for mobile mechanical arm system

Publications (2)

Publication Number Publication Date
CN116079743A true CN116079743A (en) 2023-05-09
CN116079743B CN116079743B (en) 2024-06-14

Family

ID=86202844

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310298058.8A Active CN116079743B (en) 2023-03-24 2023-03-24 Modeling and anti-overturning control method for mobile mechanical arm system

Country Status (1)

Country Link
CN (1) CN116079743B (en)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100941066B1 (en) * 2009-08-31 2010-02-09 서평원 Prestressed-precast-segmental open spendral concrete arch bridge and its constructing method
US20140188273A1 (en) * 2012-12-31 2014-07-03 King Fahd University Of Petroleum And Minerals Control method for mobile parallel manipulators
US20190321968A1 (en) * 2016-12-28 2019-10-24 Push4M Motor-drive assembly capable of deploying a traction force, use of the assembly for the motorized driving of an articulated arm, and associated method
CN111761574A (en) * 2020-05-28 2020-10-13 中联重科股份有限公司 Method and device for judging safety of operation capable of being performed by arm support and engineering machinery
CN113821935A (en) * 2021-09-30 2021-12-21 合肥工业大学 Method and system for establishing dynamic model based on symmetric constraint

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100941066B1 (en) * 2009-08-31 2010-02-09 서평원 Prestressed-precast-segmental open spendral concrete arch bridge and its constructing method
US20140188273A1 (en) * 2012-12-31 2014-07-03 King Fahd University Of Petroleum And Minerals Control method for mobile parallel manipulators
US20190321968A1 (en) * 2016-12-28 2019-10-24 Push4M Motor-drive assembly capable of deploying a traction force, use of the assembly for the motorized driving of an articulated arm, and associated method
CN111761574A (en) * 2020-05-28 2020-10-13 中联重科股份有限公司 Method and device for judging safety of operation capable of being performed by arm support and engineering machinery
CN113821935A (en) * 2021-09-30 2021-12-21 合肥工业大学 Method and system for establishing dynamic model based on symmetric constraint

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
黄康;洪健;孙顺强;: "基于轨迹约束的码垛机器人动力学分析及控制", 合肥工业大学学报(自然科学版), no. 01, 28 January 2018 (2018-01-28) *

Also Published As

Publication number Publication date
CN116079743B (en) 2024-06-14

Similar Documents

Publication Publication Date Title
CN111558941B (en) Floating base dynamics feedforward control method and device and multi-legged robot
CN113753150B (en) Control method, device and equipment of wheel leg type robot and readable storage medium
CN108089578B (en) Walking motion planning method for biped walking robot
CN111070201B (en) Reactive robust control method of quadruped robot based on ZMP theory under load mutation
CN1873572A (en) Controlling the trajectory of an effector
WO2006132330A1 (en) Equilibrium point stabilizer of legged mobile body
CN107253191B (en) Double-mechanical-arm system and coordination control method thereof
CN110371213A (en) A kind of biped robot&#39;s walking planning and control method
Park et al. Pose and posture estimation of aerial skeleton systems for outdoor flying
CN111547176B (en) Self-balancing robot control method and system, self-balancing robot and medium
CN110641738B (en) Trajectory tracking control method of space five-degree-of-freedom free flying mechanical arm
JP2010076058A (en) Control device of multiple point manipulator and method for generating operation track of hand for multiple point manipulator
Atay et al. The spherical rolling-flying vehicle: Dynamic modeling and control system design
CN113985738A (en) Gradient neural network cooperative control of non-convex constraint omnidirectional four-wheel mobile mechanical arm repetitive motion
WO2024066868A1 (en) Robot control method and apparatus, and robot, computer-readable storage medium and computer program product
CN116079743B (en) Modeling and anti-overturning control method for mobile mechanical arm system
Andaluz et al. Numerical methods for cooperative control of double mobile manipulators
Park et al. Cooperative-dual-task-space-based whole-body motion balancing for humanoid robots
Yi et al. Modeling and control of a fully-actuated quadrotor manipulator with tiltable rotors
Belguith et al. Modeling and Control of Quadrotor Transporting Cable-Suspended Load in the Longitudinal & Lateral Planes
Krafes et al. Vision-based control of a flying spherical inverted pendulum
Fuenzalida et al. Forward and inverse kinematics of a humanoid robot head for social human robot-interaction
Alghanim et al. Passivity-based adaptive controller for dynamic self-leveling of a custom-built landing platform on top of a UGV
Liu et al. Design and kinematics analysis of UPR-UPU-UR parallel vector propulsion mechanism for underwater vehicles
Chen et al. Fault-tolerant gait a quadruped robot with partially fault legs

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