CN113534817B - Dynamic modeling and track tracking control method and device for mobile robot - Google Patents
Dynamic modeling and track tracking control method and device for mobile robot Download PDFInfo
- Publication number
- CN113534817B CN113534817B CN202110953181.XA CN202110953181A CN113534817B CN 113534817 B CN113534817 B CN 113534817B CN 202110953181 A CN202110953181 A CN 202110953181A CN 113534817 B CN113534817 B CN 113534817B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- equation
- constraint
- structural
- track
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000012545 processing Methods 0.000 claims abstract description 19
- 239000011159 matrix material Substances 0.000 claims description 17
- 230000006870 function Effects 0.000 claims description 13
- 230000008859 change Effects 0.000 claims description 7
- 230000000750 progressive effect Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 13
- 238000003860 storage Methods 0.000 description 9
- 230000006872 improvement Effects 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000000452 restraining effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005055 memory storage Effects 0.000 description 1
- 229920001296 polysiloxane Polymers 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 239000010979 ruby Substances 0.000 description 1
- 229910001750 ruby Inorganic materials 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a mobile robot dynamics modeling and track tracking control method and device based on hierarchical constraint, wherein the method comprises the following steps: step 1, establishing a first dynamic equation of a mobile robot under unconstrained conditions; step 2, based on the first dynamics equation, a second-order structural constraint equation is established according to structural features of the mobile robot, structural constraint force is obtained, and a second dynamics equation with structural constraint of the mobile robot is obtained; step 3, based on the second dynamics equation, a second-order motion constraint equation is established according to the tracking track of the mobile robot, and the motion constraint force is solved according to the incompatibility problem of the ACC processing initial conditions, so that a third dynamics equation with structural constraint and motion constraint of the mobile robot is obtained; and step 4, performing track tracking control according to the third dynamic equation.
Description
Technical Field
The invention relates to the technical field of computers, in particular to a mobile robot dynamics modeling and track tracking control method and device based on hierarchical constraint.
Background
The establishment of a mobile robot model is the key for track tracking control, and the modeling method has two types: kinematic modeling and kinetic modeling. The dynamic influence is ignored in the kinematic modeling, so that the effective moving speed is reduced, and meanwhile, the gesture prediction error is caused, particularly when the moving speed of the mobile robot is high, the control signal input is obviously changed, and the gesture prediction error not only can cause extra calculation of path planning, but also can cause the mobile robot to deviate from a preset track. In order to reduce tracking errors of the mobile robot and achieve high control performance, it is necessary to build a kinetic model of the mobile robot. Since lagrangian describes motion constraints, a number of motion constraint equations based on the darabal principle have been proposed. The darabal principle is applicable in many situations but not in situations where the constraint is incomplete or non-ideal. To solve the above problems, udwadia and Kalaba propose concise, explicit and uncoupled equations of motion of the restraining force system, called Udwadia-Kalaba equations.
The biggest difference between the Udwadia-Kalaba equation and other forms of equations is that: 1. there is no need to determine the Lagrangian coefficients or other difficult to obtain parameters, which is particularly difficult to obtain for systems with incomplete constraints. In addition, the solving difficulty of the Lagrangian coefficient increases with the increase of the degree of freedom; the Udwadia-Kalaba theory does not employ linearization or approximation methods for dynamic modeling and controller design.
Disclosure of Invention
The invention aims to provide a mobile robot dynamics modeling and track tracking control method and device based on hierarchical constraint, and aims to solve the problems in the prior art.
The invention provides a mobile robot dynamics modeling and track tracking control method based on hierarchical constraint, which comprises the following steps:
step 1, establishing a first dynamic equation of a mobile robot under unconstrained conditions;
step 2, based on the first dynamics equation, a second-order structural constraint equation is established according to structural features of the mobile robot, structural constraint force is obtained, and a second dynamics equation with structural constraint of the mobile robot is obtained;
step 3, based on the second dynamics equation, a second-order motion constraint equation is established according to the tracking track of the mobile robot, and the motion constraint force is solved according to the incompatibility problem of the ACC processing initial conditions, so that a third dynamics equation with structural constraint and motion constraint of the mobile robot is obtained;
and step 4, performing track tracking control according to the third dynamic equation.
The invention provides a mobile robot dynamics modeling and track tracking control device based on hierarchical constraint, which comprises:
the first establishing module is used for establishing a first dynamics equation of the mobile robot under unconstrained conditions;
the second establishing module is used for establishing a second-order structural constraint equation according to the structural characteristics of the mobile robot based on the first dynamic equation, solving structural constraint force and obtaining a second dynamic equation with structural constraint of the mobile robot;
the third establishing module is used for establishing a second-order motion constraint equation according to the tracking track of the mobile robot based on the second dynamics equation, solving a motion constraint force according to the problem of incompatibility of initial conditions of ACC processing, and obtaining a third dynamics equation of the mobile robot with structural constraint and motion constraint;
and the track tracking control module is used for carrying out track tracking control according to the third dynamic equation.
The technical scheme of the embodiment of the invention is simple, effective and accurate, and the track tracking result is consistent with theory.
The foregoing description is only an overview of the present invention, and is intended to be implemented in accordance with the teachings of the present invention in order that the same may be more clearly understood and to make the same and other objects, features and advantages of the present invention more readily apparent.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are needed in the description of the embodiments or the prior art will be briefly described, and it is obvious that the drawings in the description below are some embodiments of the present invention, and other drawings can be obtained according to the drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a mobile robot dynamics modeling and trajectory tracking control method based on hierarchical constraints in accordance with an embodiment of the present invention;
fig. 2 is a schematic structural view of a mobile robot according to an embodiment of the present invention;
FIG. 3 is a flow chart of a mobile robot hierarchical method of an embodiment of the present invention;
fig. 4 is an overall construction diagram of a mobile robot controller according to an embodiment of the present invention;
FIG. 5 is a block diagram of a mobile robot control system according to an embodiment of the present invention;
FIG. 6 is a diagram of an embodiment C of the present invention 1 A sinusoidal graph of increasing point tracking amplitude;
FIG. 7 is a diagram of an embodiment C of the present invention 1 Schematic diagram of the error of the point and the preset track along with time;
FIG. 8 is a sinusoidal graph of incremental C-point tracking amplitude in accordance with an embodiment of the present invention;
FIG. 9 is a graph showing the error of the point C and the predetermined trajectory over time according to an embodiment of the present invention;
FIG. 10 is a schematic diagram of a mobile robot dynamics modeling and trajectory tracking control device based on hierarchical constraints in accordance with an embodiment of the present invention.
Detailed Description
The embodiment of the invention provides a mobile robot dynamics modeling and track tracking control method based on hierarchical constraint. Mechanical systems with structural and motion constraints, which may contain incomplete constraints and the number of constraint forces may be greater than 2, are difficult to obtain both their kinetic and constraint forces, and the Udwadia-Kalaba equation can solve the problems existing in the prior art.
In order to better realize track tracking control of the constraint mechanical system, the embodiment of the invention establishes a dynamic model of the mobile robot system based on Udwadia-Kalaba. In addition, a hierarchical method is proposed based on the kinetic equation of the robotic system. In this method, constraints are classified into two categories, structural constraints and motion constraints, depending on whether they vary with the trajectory. Based on the method, the dynamic equation and the constraint force of the constraint system can be obtained simultaneously. And the embodiment of the invention obtains the motion constraint by processing the incompatibility problem of the initial condition of the ACC. According to the embodiment of the invention, the three-wheeled mobile robot is selected to track the target track, and the simulation result shows that the method is simple, effective and accurate, and the track tracking result is consistent with the theory.
The technical solutions of the present invention will be clearly and completely described in connection with the embodiments, and it is apparent that the described embodiments are 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.
In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "clockwise", "counterclockwise", etc. indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings are merely for convenience in describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be configured and operated in a specific orientation, and thus should not be construed as limiting the present invention.
Furthermore, the terms "first," "second," and the like, are used for descriptive purposes only and are not to be construed as indicating or implying a relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first" or "a second" may explicitly or implicitly include one or more of the described features. In the description of the present invention, the meaning of "a plurality" is two or more, unless explicitly defined otherwise. Furthermore, the terms "mounted," "connected," "coupled," and "connected" are to be construed broadly, and may be, for example, fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the above terms in the present invention will be understood in specific cases by those of ordinary skill in the art.
Method embodiment
According to an embodiment of the present invention, a mobile robot dynamics modeling and trajectory tracking control method based on hierarchical constraint is provided, and fig. 1 is a flowchart of the mobile robot dynamics modeling and trajectory tracking control method based on hierarchical constraint according to the embodiment of the present invention, as shown in fig. 1, the mobile robot dynamics modeling and trajectory tracking control method based on hierarchical constraint according to the embodiment of the present invention specifically includes:
step 101, establishing a first kinetic equation of the mobile robot under unconstrained conditions;
102, based on the first dynamics equation, establishing a second-order structural constraint equation according to structural features of the mobile robot, solving structural constraint force, and obtaining a second dynamics equation with structural constraint of the mobile robot;
step 103, based on the second dynamics equation, a second-order motion constraint equation is established according to the tracking track of the mobile robot, and the motion constraint force is solved according to the incompatibility problem of the ACC processing initial conditions, so as to obtain a third dynamics equation of the mobile robot with structural constraint and motion constraint;
and 104, performing track tracking control according to the third dynamic equation.
The above steps of the embodiments of the present invention are specifically described below.
As shown in fig. 2, the mobile robot moves on a horizontal plane, and the coordinate system is the geodetic coordinate system XOY. The front wheels are provided with three motors for providing power for the overall driving, the overall steering and the front wheel steering of the mobile robot. The two rear wheels are driven wheels of the mobile robot, support the mobile robot and move along with the traction of the front wheels, and the wheels and the ground are in pure rolling without sliding phenomenon. Wherein (x, y) is the coordinate of C (midpoint of the connection between the two wheels of the rear wheel), (x) 1 ,y 1 ) Is C 1 θ is the direction angle of the mobile robot as a whole in the coordinate system, and ψ is the steering angle of the front wheels.
As shown in fig. 3, the specific operation steps of the mobile robot dynamics modeling and trajectory tracking control method based on hierarchical constraint are as follows:
step 1, establishing a dynamic equation of a mobile robot system under unconstrained Newton mechanics;
specifically, in step 1, the mobile robot is composed of one front wheel having driving and steering functions and two supporting rear wheels. Establishing a dynamic model of the mobile robot system under unconstrained conditions according to Newton mechanics:
wherein:
wherein: m is the mass of the mobile robot, I 1 For the moment of inertia of the mobile robot, I 2 For the moment of inertia of the front wheels of the mobile robot, θ is the direction angle of the mobile robot, ψ is the steering angle of the front wheels of the mobile robot, (x) 1 ，y 1 ) For moving the front wheel centroid C of the robot 1 In the positions of the coordinate system, Q11, Q12, Q13, Q14 are "given force".
Step 2, the relation between the speed and the angular speed of the mobile robot is controlled by the structure of the mobile robot, but is fixed along with the change of the tracking track, the relation is called structural constraint, a second-order structural constraint equation is established according to the structural characteristics of the mobile robot, and the structural constraint force is solved, so that a dynamic equation with structural constraint of the mobile robot is obtained;
specifically, in step 2, the relationship between the speed and the angular velocity of the mobile robot is controlled by the structure of the mobile robot, but is fixed as the tracking trajectory changes, thereby obtaining the first-layer constraint-structure constraint of the mobile robot.
Obtaining a first structural constraint according to the stress balance of the front wheel:
differentiating the time t yields the first second order form of the equation:
obtaining a second structural constraint according to the relation between the direction angle and the direction angular speed of the mobile robot:
wherein the mobile robot speed v is:
differentiating the time t from the (4):
wherein: ω is the direction angular velocity of the mobile robot, d is the distance from the centroid C1 of the front wheel of the mobile robot to the midpoint C of the connecting line of the two rear wheels, and v is the velocity of the front wheel of the mobile robot.
The structural constraints imposed by a mobile robot can be represented by a matrix equation, the expression of the matrix being:
wherein:
according to the Udwadia-Kalaba theory, the structural constraint force is:
wherein: b (B) s ＝AsM -1/2 ；
Obtaining
Wherein:
the mobile robot dynamics equation with structural constraints is therefore:
step 3, the constraint force required by the mobile robot to track the track changes along with the change of the track, which is called motion constraint, a second-order motion constraint equation is established according to the track of the mobile robot, and the motion constraint force is solved according to the incompatibility problem of the ACC processing initial condition, so that a dynamic equation with structural constraint and motion constraint of the mobile robot is obtained;
specifically, in step 3, the constraint force is determined according to the trajectory tracked by the mobile robot, but the constraint changes with the change of the tracked trajectory, thereby obtaining the second-layer constraint of the mobile robot, namely the motion constraint.
Front wheel centroid C of mobile robot 1 And the midpoint C of the connecting line of the two rear wheels should move on the track, because C 1 And C does not necessarily satisfy the given constraint, and the error is processed by ACC, therebyTwo motion constraints of the mobile robot are derived.
Mobile robot C 1 The points and the C point sequentially move along the track instead of repeatedly moving along part of the track to obtain a third motion constraint of the mobile robot, and three motion constraint equations are put into a matrix, wherein the matrix is as follows:
wherein:
A 11 ＝-0.1sin(0.1x 1 )-0.01x 1 cos(0.1x 1 )
A 22 ＝-0.1sin(0.1x 1 -0.1dcosθ)-0.01x 1 cos(0.1x 1 -0.1dcosθ)
+0.01dcosθcos(0.1x 1 -0.01dcosθ)
A 23 ＝-dcosθ-0.01dx 1 sinθcos(0.1x 1 -0.1dcosθ)-0.1dsinθsin(0.1x 1 -0.1dcosθ)
+0.01d 2 sinθcosθcos(0.1x 1 -0.1dcosθ)
and 4, establishing a mobile robot system dynamics modeling and track tracking control method based on hierarchical constraint, verifying whether the mobile robot system dynamics modeling and track tracking control method meets the system stability requirement, ending the design if the mobile robot system dynamics modeling and track tracking control method meets the system stability requirement, and re-designing the method if the mobile robot system dynamics modeling and track tracking control method does not meet the system stability requirement.
Specifically, in step 4, the mobile robot is provided with driving, steering and front wheel steering by three motors respectively, and obtains a motion constraint force according to the Udwadia-Kalaba theory:
in the method, in the process of the invention,
can be written as: />
Wherein, C is a constant matrix:
u is the control input force:
in the control input force u, F x As a component of force in the x-axis, F y Is the component of the force in the y-axis. U (U) θ U is the torque of the mobile robot ψ Is the front wheel torque of the mobile robot.
Driving moment U of mobile robot d The method comprises the following steps:
thus controlling the restraining force
Under the constraint of the structure and the motion of the unconstrained mobile robot, the kinetic equation is as follows:
when the mobile robot moves along the preset track, driving torque and steering torque of the three motors are obtained according to the step 3, and closed-loop feedback control is formed by combining the actual torque of the motors, so that the mobile robot moves along the preset track.
As shown in fig. 4, the input quantity of the controller is a preset track given by the mobile robot, the control force required by the driving motor and the steering motor of the mobile robot is calculated by the tracking controller of the mobile robot, and the control force is regulated by the torque controller in a closed-loop control manner, so that the mobile robot moves according to the preset track.
As shown in fig. 5, the mobile robot system is equipped with a gyroscope, an accelerometer sensor, a current sensor, a motor driver, a photoelectric encoder, and the like. The system controls the mobile robot to advance, turn and turn by a main controller, and the main controller respectively sends instructions to three motor drivers to drive the motors to work. The position information of the mobile robot is acquired by a photoelectric encoder and fed back to a motor driver. The mobile robot records current information through a current sensor and calculates the actual moment born by the mobile robot. The motor driver compares the actual moment with the given moment to adjust the motor of the mobile robot, so that the mobile robot moves according to a preset track.
Fig. 6 and 8 are sinusoidal graphs with increasing tracking amplitude at points C1 and C, respectively, in accordance with the present invention. The preset track is a sine curve with linearly increased amplitude, the parameters of the mobile robot controller are set to follow the preset track, the actual motion track of the mobile robot is shown as a dotted line, and the preset track is shown as a solid line. From the figure, the true track and the preset track of the mobile robot are very consistent, the following effect is very perfect, and the control method is proved to be very effective.
Fig. 7 and 9 are graphs showing the error of the point C1 and the point C with respect to the predetermined trajectory according to the present invention, respectively. Because the initial conditions may not meet the constraint conditions, the Udwadia-Kalaba theory can solve the incompatible track tracking control of the initial conditions through ACC processing. As clearly seen in the figure, the error e varies with time 21 And e 22 Stable near zero. It has been demonstrated that based on this method, the mobile robot can very accurately track the preset trajectory.
The steering angle of the front wheel and the overall steering angle of the mobile robot are respectively calculated by motor angle information obtained by position sensors of photoelectric encoders of two rotating motors of the front wheel, and the direction angle theta of the mobile robot can be acquired and calculated by a gyroscope and an accelerometer sensor. U (U) d 、U θ 、U ψ Can be calculated by using the formulas (12), (13) and (14). Table 1 shows the meanings and assignment of robot parameters in this example, and Table 2 shows the meanings and units of variables in the examples of the present invention.
In summary, in order to better realize the track tracking control of the constrained mechanical system, the embodiment of the invention establishes a dynamic model of the unconstrained mechanical system; based on a dynamics model, a hierarchical constraint method is provided. In this method, the constraint is changed according to whether the tracking trajectory is changed.
TABLE 1
| Parameters (parameters) | Value of |
| Mobile robot mass | m＝800kg |
| Mobile robot rotation inertia | I 1 ＝333kg·m 2 |
| Front wheel rotation inertia | I 2 ＝1kg·m 2 |
| Radius of wheel | r＝0.2m |
| C 1 Distance to C | d＝1m |
TABLE 2
They are divided into two classes, structural constraints and motion constraints. Based on the method, the dynamic equation and the constraint force of the constraint system can be obtained simultaneously. Processing the situation that the initial constraint of the mobile robot does not meet the Udwadia-Kalaba theory through the ACC, thereby obtaining motion constraint; a three-wheeled mobile robot is selected to track a preset track, and simulation results show that the method is simple, effective and accurate, and the result is consistent with theory.
Device embodiment
According to an embodiment of the present invention, there is provided a mobile robot dynamics modeling and trajectory tracking control device based on hierarchical constraint, and fig. 10 is a schematic diagram of the mobile robot dynamics modeling and trajectory tracking control device based on hierarchical constraint according to the embodiment of the present invention, as shown in fig. 10, where the mobile robot dynamics modeling and trajectory tracking control device based on hierarchical constraint according to the embodiment of the present invention specifically includes:
a first establishing module 10 for establishing a first kinetic equation of the mobile robot without constraint; the first establishing module 10 is specifically configured to:
according to Newton mechanics, a first dynamics model of the mobile robot system without constraint is established based on the formula 1-4:
wherein the mobile robot comprises a front wheel with driving and steering functions and two supporting rear wheels, m is the mass of the mobile robot, I 1 For the moment of inertia of the mobile robot, I 2 For the moment of inertia of the front wheels of the mobile robot, θ is the direction angle of the mobile robot, ψ is the steering angle of the front wheels of the mobile robot, (x) 1 ，y 1 ) For moving the front wheel centroid C of the robot 1 In the position of the coordinate system, Q 11 、Q 12 、Q 13 、Q 14 For a given force.
A second establishing module 12, configured to establish a second-order structural constraint equation according to structural features of the mobile robot based on the first kinetic equation, and calculate structural constraint force to obtain a second kinetic equation with structural constraint of the mobile robot;
the second establishing module 12 is specifically configured to:
obtaining a first structural constraint according to the front wheel stress balance of the mobile robot:
differentiating the time t yields the first second order form of the equation:
obtaining a second structural constraint according to the relation between the direction angle and the direction angular speed of the mobile robot:
wherein, mobile robot speed v is:
differentiating equation 7 with respect to time t yields equation 9:
wherein ω is the angular velocity of the mobile robot, and d is the centroid C of the front wheel of the mobile robot 1 The distance from the connecting line midpoint C of the two rear wheels is v, which is the speed of the front wheels of the mobile robot;
determining structural constraints imposed by the mobile robot according to the matrix equation set forth in equations 10-12:
according to Udwadia-Kalaba theory, structural constraint forces are determined according to formulas 13-15:
B s ＝A 3 M -1/2 equation 14;
wherein,,
the second kinetic equation for a mobile robot with structural constraints is determined according to equation 16:
a third establishing module 14, configured to establish a second-order motion constraint equation according to a tracking track of the mobile robot based on the second dynamic equation, and calculate a motion constraint force according to the problem of incompatibility of the ACC processing initial conditions, so as to obtain a third dynamic equation of the mobile robot with structural constraint and motion constraint; the third setup module 14 is specifically configured to:
determining a second layer constraint of the mobile robot system, namely a first motion constraint equation, according to the change of the tracking track of the mobile robot system;
front wheel centroid C based on mobile robot system 1 The midpoint C of the connecting line of the two rear wheels moves on the track, and ACC processing is adopted to obtain a second motion constraint equation of the mobile robot system;
based on mobile robot C 1 The point and the point C sequentially move along the track to obtain a third motion constraint equation of the mobile robot system;
three motion constraint equations are put into a matrix, and the matrix expression is:
wherein,,
based on the mobile robot, three motors respectively provide driving, steering and front wheel steering, and motion constraint force is obtained according to the Udwadia-Kalaba theory:
will beWritten as equation 20:
wherein C is a constant matrix, and u is a control input force; in the control input force u, F x As a component of force in the x-axis, F y As a component of the force in the y-axis, U θ U is the torque of the mobile robot ψ Is the front wheel torque of the mobile robot.
Determining a drive moment U of a mobile robot d The method comprises the following steps:
control constraint force is determined according to equation 24:
the third dynamic equation of the unconstrained mobile robot system under the structural constraint and the motion constraint is determined as follows:
a track following control module 16 for performing track following control according to the third dynamic equation
When the mobile robot moves along the preset track, driving torque and steering torque of three motors are obtained according to the third dynamic equation, and closed-loop feedback control is formed by combining the actual torque of the motors, so that the mobile robot moves along the preset track.
The embodiment of the present invention is an embodiment of a device corresponding to the embodiment of the method, and specific operations of each module may be understood by referring to descriptions of the embodiment of the method, which are not repeated herein.
The foregoing describes specific embodiments of the present disclosure. Other embodiments are within the scope of the following claims. In some cases, the actions or steps recited in the claims can be performed in a different order than in the embodiments and still achieve desirable results. In addition, the processes depicted in the accompanying figures do not necessarily require the particular order shown, or sequential order, to achieve desirable results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.
In the 30 s of the 20 th century, improvements to one technology could clearly be distinguished as improvements in hardware (e.g., improvements to circuit structures such as diodes, transistors, switches, etc.) or software (improvements to the process flow). However, with the development of technology, many improvements of the current method flows can be regarded as direct improvements of hardware circuit structures. Designers almost always obtain corresponding hardware circuit structures by programming improved method flows into hardware circuits. Therefore, an improvement of a method flow cannot be said to be realized by a hardware entity module. For example, a programmable logic device (Programmable Logic Device, PLD) (e.g., field programmable gate array (Field Programmable Gate Array, FPGA)) is an integrated circuit whose logic function is determined by the programming of the device by a user. A designer programs to "integrate" a digital system onto a PLD without requiring the chip manufacturer to design and fabricate application-specific integrated circuit chips. Moreover, nowadays, instead of manually manufacturing integrated circuit chips, such programming is mostly implemented by using "logic compiler" software, which is similar to the software compiler used in program development and writing, and the original code before the compiling is also written in a specific programming language, which is called hardware description language (Hardware Description Language, HDL), but not just one of the hdds, but a plurality of kinds, such as ABEL (Advanced Boolean Expression Language), AHDL (Altera Hardware Description Language), confluence, CUPL (Cornell University Programming Language), HDCal, JHDL (Java Hardware Description Language), lava, lola, myHDL, PALASM, RHDL (Ruby Hardware Description Language), etc., VHDL (Very-High-Speed Integrated Circuit Hardware Description Language) and Verilog are currently most commonly used. It will also be apparent to those skilled in the art that a hardware circuit implementing the logic method flow can be readily obtained by merely slightly programming the method flow into an integrated circuit using several of the hardware description languages described above.
The controller may be implemented in any suitable manner, for example, the controller may take the form of, for example, a microprocessor or processor and a computer readable medium storing computer readable program code (e.g., software or firmware) executable by the (micro) processor, logic gates, switches, application specific integrated circuits (Application Specific Integrated Circuit, ASIC), programmable logic controllers, and embedded microcontrollers, examples of which include, but are not limited to, the following microcontrollers: ARC 625D, atmel AT91SAM, microchip PIC18F26K20, and Silicone Labs C8051F320, the memory controller may also be implemented as part of the control logic of the memory. Those skilled in the art will also appreciate that, in addition to implementing the controller in a pure computer readable program code, it is well possible to implement the same functionality by logically programming the method steps such that the controller is in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc. Such a controller may thus be regarded as a kind of hardware component, and means for performing various functions included therein may also be regarded as structures within the hardware component. Or even means for achieving the various functions may be regarded as either software modules implementing the methods or structures within hardware components.
The system, apparatus, module or unit set forth in the above embodiments may be implemented in particular by a computer chip or entity, or by a product having a certain function. One typical implementation is a computer. In particular, the computer may be, for example, a personal computer, a laptop computer, a cellular telephone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.
For convenience of description, the above devices are described as being functionally divided into various units, respectively. Of course, the functions of each unit may be implemented in the same piece or pieces of software and/or hardware when implementing the embodiments of the present specification.
One skilled in the relevant art will recognize that one or more embodiments of the present description may be provided as a method, system, or computer program product. Accordingly, one or more embodiments of the present description may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present description can take the form of a computer program product on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) having computer-usable program code embodied therein.
The present description is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the specification. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
In one typical configuration, a computing device includes one or more processors (CPUs), input/output interfaces, network interfaces, and memory.
The memory may include volatile memory in a computer-readable medium, random Access Memory (RAM) and/or nonvolatile memory, such as Read Only Memory (ROM) or flash memory (flash RAM). Memory is an example of computer-readable media.
Computer readable media, including both non-transitory and non-transitory, removable and non-removable media, may implement information storage by any method or technology. The information may be computer readable instructions, data structures, modules of a program, or other data. Examples of storage media for a computer include, but are not limited to, phase change memory (PRAM), static Random Access Memory (SRAM), dynamic Random Access Memory (DRAM), other types of Random Access Memory (RAM), read Only Memory (ROM), electrically Erasable Programmable Read Only Memory (EEPROM), flash memory or other memory technology, compact disc read only memory (CD-ROM), digital Versatile Discs (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices, or any other non-transmission medium, which can be used to store information that can be accessed by a computing device. Computer-readable media, as defined herein, does not include transitory computer-readable media (transmission media), such as modulated data signals and carrier waves.
It should also be noted that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article or apparatus that comprises the element.
One or more embodiments of the present description may be described in the general context of computer-executable instructions, such as program modules, including, generally, routines, programs, objects, components, data structures, etc., that perform particular tasks or implement particular abstract data types; one or more embodiments of the specification may also be practiced in distributed computing environments where tasks are performed by remote processing devices that are linked through a communications network. In a distributed computing environment, program modules may be located in both local and remote computer storage media including memory storage devices.
In this specification, each embodiment is described in a progressive manner, and identical and similar parts of each embodiment are all referred to each other, and each embodiment mainly describes differences from other embodiments. In particular, for system embodiments, since they are substantially similar to method embodiments, the description is relatively simple, as relevant to see a section of the description of method embodiments.
The foregoing description is by way of example only and is not intended to limit the present disclosure. Various modifications and changes may occur to those skilled in the art. Any modifications, equivalent substitutions, improvements, etc. that fall within the spirit and principles of the present document are intended to be included within the scope of the claims of the present document.
Claims (4)
1. The mobile robot dynamics modeling and track tracking control method based on hierarchical constraint is characterized by comprising the following steps of:
step 1, establishing a first dynamic equation of a mobile robot under unconstrained conditions;
step 2, based on the first dynamics equation, a second-order structural constraint equation is established according to structural features of the mobile robot, structural constraint force is obtained, and a second dynamics equation with structural constraint of the mobile robot is obtained;
step 3, based on the second dynamics equation, a second-order motion constraint equation is established according to a tracking track of the mobile robot, an initial condition incompatibility problem is processed according to a progressive convergence criterion ACC, a motion constraint force is obtained, and a third dynamics equation of the mobile robot with structural constraint and motion constraint is obtained;
step 4, track tracking control is carried out according to the third dynamic equation;
the step 1 specifically includes:
according to Newton mechanics, a first dynamics model of the mobile robot system without constraint is established based on the formula 1-4:
wherein the mobile robot comprises a front wheel with driving and steering functions and two supporting rear wheels, m is the mass of the mobile robot, I 1 For the moment of inertia of the mobile robot, I 2 For the moment of inertia of the front wheels of the mobile robot, θ is the direction angle of the mobile robot, ψ is the steering angle of the front wheels of the mobile robot, (x) 1 ，y 1 ) For moving the front wheel centroid C of the robot 1 In the position of the coordinate system, Q 11 、Q 12 、Q 13 、Q 14 For a given force;
the step 2 specifically includes:
obtaining a first structural constraint according to the front wheel stress balance of the mobile robot:
differentiating the time t yields the first second order form of the equation:
obtaining a second structural constraint according to the relation between the direction angle and the direction angular speed of the mobile robot:
wherein, mobile robot speed v is:
differentiating equation 7 with respect to time t yields equation 9:
wherein ω is the angular velocity of the mobile robot, and d is the centroid C of the front wheel of the mobile robot 1 The distance from the connecting line midpoint C of the two rear wheels is v, which is the speed of the front wheels of the mobile robot;
determining structural constraints imposed by the mobile robot according to the matrix equation set forth in equations 10-12:
according to Udwadia-Kalaba theory, structural constraint forces are determined according to formulas 13-15:
B s ＝A s M -1/2 equation 14;
wherein,,
the second kinetic equation for a mobile robot with structural constraints is determined according to equation 16:
the step 3 specifically includes:
determining a second layer constraint of the mobile robot system, namely a first motion constraint equation, according to the change of the tracking track of the mobile robot system;
front wheel centroid C based on mobile robot system 1 The midpoint C of the connecting line of the two rear wheels moves on the track, and ACC processing is adopted to obtain a second motion constraint equation of the mobile robot system;
based on mobile robot C 1 The point and the point C sequentially move along the track to obtain a third motion constraint equation of the mobile robot system;
three motion constraint equations are put into a matrix, and the matrix expression is:
wherein,,
A 11 ＝-0.1sin(0.1x 1 )-0.01x 1 cos(0.1x 1 )
A 22 ＝-0.1sin(0.1x 1 -0.1dcosθ)-0.01x 1 cos(0.1x 1 -0.1dcosθ)+0.01dcosθcos(0.1x 1 -0.01dcosθ)
A 23 ＝-dcosθ-0.01dx 1 sinθcos(0.1x 1 -0.1dcosθ)-0.1dsinθsin(0.1x 1 -0.1dcosθ)+0.01d 2 sinθcosθcos(0.1x 1 -0.1dcosθ)
based on the mobile robot, three motors respectively provide driving, steering and front wheel steering, and motion constraint force is obtained according to the Udwadia-Kalaba theory:
will beWritten as equation 20:
wherein C is a constant matrix, and u is a control input force; in the control input force u, F x As a component of force in the x-axis, F y As a component of the force in the y-axis, U θ U is the torque of the mobile robot ψ Front wheel torque for mobile robot;
determining a drive moment U of a mobile robot d The method comprises the following steps:
control constraint force is determined according to equation 24:
the third dynamic equation of the unconstrained mobile robot system under the structural constraint and the motion constraint is determined as follows:
2. the method according to claim 1, wherein the step 4 specifically comprises:
when the mobile robot moves along the preset track, driving torque and steering torque of three motors are obtained according to the third dynamic equation, and closed-loop feedback control is formed by combining the actual torque of the motors, so that the mobile robot moves along the preset track.
3. A mobile robot dynamics modeling and trajectory tracking control device based on hierarchical constraints, comprising:
the first establishing module is used for establishing a first dynamics equation of the mobile robot under unconstrained conditions;
the second establishing module is used for establishing a second-order structural constraint equation according to the structural characteristics of the mobile robot based on the first dynamic equation, solving structural constraint force and obtaining a second dynamic equation with structural constraint of the mobile robot;
the third establishing module is used for establishing a second-order motion constraint equation according to the tracking track of the mobile robot based on the second dynamics equation, solving a motion constraint force according to the problem of incompatibility of initial conditions of ACC processing, and obtaining a third dynamics equation of the mobile robot with structural constraint and motion constraint;
the track tracking control module is used for carrying out track tracking control according to the third dynamic equation;
the first establishing module is specifically configured to:
according to Newton mechanics, a first dynamics model of the mobile robot system without constraint is established based on the formula 1-4:
wherein the mobile robot comprises a front wheel with driving and steering functions and two supporting rear wheels, m is the mass of the mobile robot, I 1 For the moment of inertia of the mobile robot, I 2 For the moment of inertia of the front wheels of the mobile robot, θ is the direction angle of the mobile robot, ψ is the steering angle of the front wheels of the mobile robot, (x) 1 ，y 1 ) For moving the front wheel centroid C of the robot 1 In the position of the coordinate system, Q 11 、Q 12 、Q 13 、Q 14 For a given force;
the second establishing module is specifically configured to:
obtaining a first structural constraint according to the front wheel stress balance of the mobile robot:
differentiating the time t yields the first second order form of the equation:
obtaining a second structural constraint according to the relation between the direction angle and the direction angular speed of the mobile robot:
wherein, mobile robot speed v is:
differentiating equation 7 with respect to time t yields equation 9:
wherein ω is the angular velocity of the mobile robot, and d is the centroid C of the front wheel of the mobile robot 1 The distance from the connecting line midpoint C of the two rear wheels is v, which is the speed of the front wheels of the mobile robot;
determining structural constraints imposed by the mobile robot according to the matrix equation set forth in equations 10-12:
according to Udwadia-Kalaba theory, structural constraint forces are determined according to formulas 13-15:
B s ＝A s M -1/2 equation 14;
wherein,,
the second kinetic equation for a mobile robot with structural constraints is determined according to equation 16:
the third establishing module is specifically configured to:
determining a second layer constraint of the mobile robot system, namely a first motion constraint equation, according to the change of the tracking track of the mobile robot system;
front wheel centroid C based on mobile robot system 1 The midpoint C of the connecting line of the two rear wheels moves on the track, and ACC processing is adopted to obtain a second motion constraint equation of the mobile robot system;
based on mobile robot C 1 The point and the point C sequentially move along the track to obtain a third motion constraint equation of the mobile robot system;
three motion constraint equations are put into a matrix, and the matrix expression is:
wherein,,
A 11 ＝-0.1sin(0.1x 1 )-0.01x 1 cos(0.1x 1 )
A 22 ＝-0.1sin(0.1x 1 -0.1dcosθ)-0.01x 1 cos(0.1x 1 -0.1dcosθ)+0.01dcosθcos(0.1x 1 -0.01dcosθ)
A 23 ＝-dcosθ-0.01dx 1 sinθcos(0.1x 1 -0.1dcosθ)-0.1dsinθsin(0.1x 1 -0.1dcosθ)+0.01d 2 sinθcosθcos(0.1x 1 -0.1dcosθ)
based on the mobile robot, three motors respectively provide driving, steering and front wheel steering, and motion constraint force is obtained according to the Udwadia-Kalaba theory:
will beWritten as equation 20:
wherein C is a constant matrix, and u is a control input force; in the control input force u, F x As a component of force in the x-axis, F y As a component of the force in the y-axis, U θ U is the torque of the mobile robot ψ Front wheel torque for mobile robot;
determining a drive moment U of a mobile robot d The method comprises the following steps:
control constraint force is determined according to equation 24:
the third dynamic equation of the unconstrained mobile robot system under the structural constraint and the motion constraint is determined as follows:
4. the apparatus of claim 3, wherein the trajectory tracking control module is specifically configured to:
when the mobile robot moves along the preset track, driving torque and steering torque of three motors are obtained according to the third dynamic equation, and closed-loop feedback control is formed by combining the actual torque of the motors, so that the mobile robot moves along the preset track.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110953181.XA CN113534817B (en) | 2021-08-19 | 2021-08-19 | Dynamic modeling and track tracking control method and device for mobile robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110953181.XA CN113534817B (en) | 2021-08-19 | 2021-08-19 | Dynamic modeling and track tracking control method and device for mobile robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113534817A CN113534817A (en) | 2021-10-22 |
| CN113534817B true CN113534817B (en) | 2023-07-28 |
Family
ID=78091279
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110953181.XA Active CN113534817B (en) | 2021-08-19 | 2021-08-19 | Dynamic modeling and track tracking control method and device for mobile robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113534817B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114571461B (en) * | 2022-03-24 | 2023-08-11 | 合肥工业大学 | Three-degree-of-freedom three-dimensional parallel robot track tracking control algorithm based on Udwadia-Kalaba method |
| CN115321434B (en) * | 2022-08-05 | 2023-12-26 | 浙江华睿科技股份有限公司 | Steering control method and device for forklift |
| CN117075604B (en) * | 2023-08-18 | 2024-08-02 | 山东科技大学 | Wheel type mobile robot control method based on generalized performance measurement |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1895720A1 (en) * | 2006-08-30 | 2008-03-05 | Mitsubishi Electric Corporation | Computer implemented method for finding optimal path from source to destination |
| CN108459605A (en) * | 2018-03-22 | 2018-08-28 | 合肥工业大学 | Trajectory Tracking Control method based on AGV system |
| EP3792125A1 (en) * | 2019-09-13 | 2021-03-17 | Zenuity AB | Safe trajectory tracking in uncertain environments |
| CN112578814A (en) * | 2020-12-15 | 2021-03-30 | 西北工业大学 | Linear track tracking control method for formation of multiple autonomous underwater vehicles |
-
2021
- 2021-08-19 CN CN202110953181.XA patent/CN113534817B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1895720A1 (en) * | 2006-08-30 | 2008-03-05 | Mitsubishi Electric Corporation | Computer implemented method for finding optimal path from source to destination |
| CN108459605A (en) * | 2018-03-22 | 2018-08-28 | 合肥工业大学 | Trajectory Tracking Control method based on AGV system |
| EP3792125A1 (en) * | 2019-09-13 | 2021-03-17 | Zenuity AB | Safe trajectory tracking in uncertain environments |
| CN112578814A (en) * | 2020-12-15 | 2021-03-30 | 西北工业大学 | Linear track tracking control method for formation of multiple autonomous underwater vehicles |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113534817A (en) | 2021-10-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113534817B (en) | Dynamic modeling and track tracking control method and device for mobile robot | |
| CN114167721B (en) | Linear active disturbance rejection track tracking control method and controller for manned robot | |
| CN103197673A (en) | Robot motion track locating method and robot motion track locating device | |
| Barnett et al. | Time-Optimal Trajectory Planning of Cable-Driven Parallel Mechanisms for Fully Specified Paths With G 1-Discontinuities | |
| CN111062372B (en) | Method and device for predicting obstacle track | |
| CN111797906B (en) | Method and device for positioning based on vision and inertial mileage | |
| Kim | Trajectory generation of a two-wheeled mobile robot in an uncertain environment | |
| CN108453741B (en) | Flexible servo control method for industrial robot | |
| CN116661293B (en) | Movement control method and system for wheel-leg type vehicle jumping obstacle | |
| CN111547176B (en) | Self-balancing robot control method and system, self-balancing robot and medium | |
| Vista et al. | Design convergent Dynamic Window Approach for quadrotor navigation | |
| Min et al. | Comparative study of ROS on embedded system for a mobile robot | |
| Luo et al. | Adaptive hybrid impedance control algorithm based on subsystem dynamics model for robot polishing | |
| CN116176603A (en) | Method, device and equipment for determining course angle of vehicle | |
| CN116929407A (en) | Self-adaptive data calibration method and device | |
| CN114371715A (en) | Control method and control device of bearing equipment | |
| Ihnatsiuk et al. | CONSTRUCTION OF A MATHEMATICAL MODEL OF THE MOVEMENT OF A FOUR-WHEELED MOBILE ROBOT | |
| CN113985889B (en) | Track planning method and device, storage medium and electronic equipment | |
| CN114148350B (en) | Unmanned equipment control method and device | |
| Li et al. | Sliding Mode Control (SMC) of Image‐Based Visual Servoing for a 6DOF Manipulator | |
| CN112925327A (en) | Two-dimensional code inertial navigation control algorithm | |
| Savnani et al. | Modelling, Design and Control of a Four wheel Holonomic Drive | |
| CN112572473B (en) | Control method and device of unmanned equipment | |
| CN113607080B (en) | Method and system for realizing inertial space scanning imaging | |
| CN116126002A (en) | Six-foot robot walking track planning based on control of body speed and angular speed |
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 |