CN117724472A - Mobile robot track tracking control method and system of kinematic model - Google Patents
Mobile robot track tracking control method and system of kinematic model Download PDFInfo
- Publication number
- CN117724472A CN117724472A CN202311473092.0A CN202311473092A CN117724472A CN 117724472 A CN117724472 A CN 117724472A CN 202311473092 A CN202311473092 A CN 202311473092A CN 117724472 A CN117724472 A CN 117724472A
- Authority
- CN
- China
- Prior art keywords
- robot
- center
- error
- speed
- model
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000033001 locomotion Effects 0.000 claims abstract description 26
- 230000005484 gravity Effects 0.000 claims description 46
- 238000009826 distribution Methods 0.000 claims description 25
- 238000004364 calculation method Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 7
- 239000012636 effector Substances 0.000 claims description 6
- 238000004590 computer program Methods 0.000 claims description 2
- 238000012545 processing Methods 0.000 claims description 2
- 230000015572 biosynthetic process Effects 0.000 description 18
- 238000005755 formation reaction Methods 0.000 description 18
- 238000004422 calculation algorithm Methods 0.000 description 17
- 238000004088 simulation Methods 0.000 description 16
- 238000013461 design Methods 0.000 description 9
- 238000005516 engineering process Methods 0.000 description 5
- 238000004519 manufacturing process Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 238000011217 control strategy Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000003860 storage Methods 0.000 description 2
- 210000001258 synovial membrane Anatomy 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009429 electrical wiring Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000008713 feedback mechanism Effects 0.000 description 1
- 230000000977 initiatory effect Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000014759 maintenance of location Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000013468 resource allocation Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Landscapes
- Numerical Control (AREA)
Abstract
The invention discloses a mobile robot track tracking control method and a system of a kinematic model, wherein the scheme firstly obtains the kinematic model of an incomplete wheeled robot according to the motion characteristics of the incomplete wheeled robot; then, establishing a track tracking error differential equation according to the error between the actual pose and the expected pose of the mobile robot; and finally, constructing a robot track tracking controller according to a track tracking error differential equation so as to realize track tracking of the incomplete wheel type mobile robot. The control scheme is more robust and high in control precision, and can effectively overcome the problems existing in the prior art.
Description
Technical Field
The invention relates to a mobile robot control technology, in particular to a mobile robot track tracking control technology.
Background
The track tracking control scheme of the existing wheel type mobile robot mainly relates to the technologies of path planning, controllers, track tracking algorithms, kinematic models and real-time sensor feedback.
Wherein path planning is the first step of trajectory tracking control, which determines the ideal trajectory that the robot needs to follow. Global paths are typically generated using global path planning algorithms (e.g., a-algorithm, dijkstra algorithm), and then smooth local trajectories are generated using local path planning algorithms (e.g., speed-based Spline interpolation).
The controller is used as a core part and is mainly responsible for generating proper control input according to the current state and the target track so as to enable the robot to move according to the planned track. The controller may be designed based on different control strategies, such as PID control, model Predictive Control (MPC), or adaptive control.
The trajectory tracking algorithm is used as part of the controller to calculate the error between the current position of the robot and the trajectory. These algorithms typically include offset calculation, direction error calculation, and speed error calculation.
The kinematic model describes the dynamics and constraints of the robot. These models typically include kinematic constraints of the wheeled robot, such as speed limits, turning radii, etc.
The real-time sensor feedback is mainly constructed by corresponding sensors, such as a laser radar, a camera, an encoder and the like, and is used for monitoring the position and the surrounding environment of the robot in real time, and the data of the sensors are used for real-time feedback of a controller so as to correct the track tracking of the robot.
The track tracking control scheme of the existing wheel type mobile robot is implemented in a specific implementation mode, and has a certain problem:
firstly, the existing track tracking control scheme is relatively complex in structure, and high-expertise control algorithm design and parameter adjustment are needed, so that the practical application scheme is too complex, the maintenance cost is reduced, and the practicability is reduced.
Furthermore, existing trajectory tracking control schemes require sensors to sense the surrounding environment, which may lead to tracking errors if sensor noise, occlusion and imperfections in the complex environment.
Furthermore, existing trajectory tracking control schemes become more difficult to track robot trajectories at high speeds, requiring higher performance control algorithms and sensors to ensure safety and accuracy.
The Chinese patent application publication No. CN115933647A discloses an OMR track tracking control method based on a compound control algorithm, which comprises the following steps: giving the gesture and the speed of a reference robot, calculating the position information of the reference robot through a nominal kinematic model, and obtaining the position information of a following robot through an actual kinematic model; combining the track tracking error model, and calculating to obtain a tracking error of the following robot at the kth moment; obtaining the optimal input of the following robot through a model predictive controller, and obtaining an estimated value of a real lumped disturbance vector of the following robot by combining an improved sliding mode observer; and carrying out disturbance estimation compensation on the following robot through the estimated value, and controlling the speed of the following robot.
The scheme is specifically that model prediction is combined with a synovial membrane control technology, and when the method is applied practically, the method has the defects of smoothness, robustness, target tracking and the like, and cannot meet the actual requirements.
Disclosure of Invention
Aiming at the problems of the existing mobile robot track tracking control scheme, the invention aims to provide a mobile robot track tracking control method based on a kinematic model, and the control method is realized by solving a nonlinear system based on a back-stepping method, has higher robustness and high control precision, and can effectively overcome the problems in the prior art; on the basis, the invention further provides a mobile robot track tracking control system based on the kinematic model.
In order to achieve the above object, the method for controlling tracking of a mobile robot trajectory based on a kinematic model according to the present invention includes:
1) Distinguishing the relationship between the center of gravity and the shape center of the robot, and establishing a robot kinematic model;
2) Based on a kinematic model of the robot, a tracking error model of the robot is established according to the error between the actual pose and the expected pose of the mobile robot, and a tracking error differential equation of the robot track is calculated.
3) Generating a robot speed controller according to the error differential equation obtained in the step 2).
In some embodiments of the present invention, when the robot kinematic model is built in step 1), it is first determined that the center of gravity and the body center of the robot are separately determined, and based on this, the relative relationship between the center of gravity and the body center of the robot is further determined, and then the robot kinematic model is built based on the relative relationship between the center of gravity and the body center of the robot.
In some embodiments of the invention, when the robot kinematic model is built in step 1),
the method comprises the following steps:
(1.1) robot geometric modeling:
firstly, modeling a geometric structure of a robot;
(1.2) modeling of mass distribution:
modeling mass distribution of the robot;
(1.3) center of gravity position calculation:
based on mass distribution modeling of the robot, calculating and determining the gravity center position of the whole robot system;
(1.4) calculation of body center:
geometric modeling is carried out on the robot, the geometric center point counting position of the external shape of the robot is determined, and mass distribution is not considered;
(1.5) distinguishing center of gravity from body center:
the relation between the center of gravity and the center of the body is distinguished by calculating the relative position between the center of gravity and the center of the body;
(1.6) establishing a kinematic model:
and establishing a kinematic model of the robot based on the relation between the gravity center and the shape center of the robot.
In some embodiments of the invention, building a robot tracking error model in step 2) comprises:
(2.1) constructing a global coordinate system;
(2.2) determining in a global coordinate system: pose coordinate R of pilot robot L ＝(x L ,y L ,θ L ) T Pose coordinate R of virtual robot V ＝(x V ,y V ,θ V ) T Following robot R F Pose coordinates R of position F ＝(x F ,y F ,θ F ) T ，
Wherein, (x) L ,y L ,θ L ) T ,x L ,y L Respectively representing the coordinates of the piloting robot in the x axis and the y axis in the global coordinate system, theta L Is the yaw angle of the robot;
(x V ,y V ,θ V ) T ,x V ,y V respectively representing the coordinates of the virtual robot in the x axis and the y axis in the global coordinate system, theta V Is the yaw angle of the robot;
(x F ,y F ,θ F ) T ,x F ,y F respectively representing the coordinates of the following robot in the x-axis and the y-axis in the global coordinate system, theta F Is the yaw angle of the robot;
(2.3) determining the virtual robot R according to the pose coordinates determined in the step (2.2) V Relative to piloting robot R L Is a model of the actual position of:
determining a following robot R F Relative to piloting robot R L Is a model of the actual position of:
wherein L is d To follow the robot R F And pilot robot R L Is a desired relative distance of (2);
is a desired relative angle;
L c to follow the robot R in the global coordinate system F And pilot robot R L Is used for the distance of the object(s),to follow the robot R in the global coordinate system F And pilot robot R L Is provided with->
(2.4) constructing a dynamic error model under a following robot coordinate system based on the robot kinematics model established in the step (1):
wherein, (x) V -x F ,y V -y F ,θ V -θ F ) T To follow the robot R in world coordinate system oxy F And virtual robot R V Pose error of (2);
(2.5) calculating and determining a robot track tracking error differential equation based on the dynamic error model constructed in the step (2.4):
in some embodiments of the invention, the robot speed controller is generated in step 3) in a back-stepping manner based on the error differential equation obtained in step 2).
In some embodiments of the invention, the robot speed controller formed in step 3) is as follows:
wherein v is F To follow the robot R F Linear velocity, omega F To follow the robot R F Is a function of the angular velocity of the rotor.
In some embodiments of the invention, the trajectory tracking control method
A process for controlling a robot to follow a target speed adjustment feedback based on a robot speed controller, comprising:
(3.1) calculating a motion error:
when the robot starts to execute the task, measuring the actual speed and the position, and calculating the error between the actual speed and the target speed to serve as a basis of feedback control;
(3.2) designing a speed controller:
constructing a speed controller based on the established speed controller model according to the speed error, the speed controller generating a control input to adjust the speed of the robot;
(3.3) feedback speed control:
implementing the output of the speed controller, adjusting the joint speed and the end effector speed of the robot to cause the robot to trend toward the target speed;
(3.4) real-time feedback:
the actual speed and position are measured continuously, the speed error is recalculated, and the output of the speed controller is adjusted according to the error, so that continuous feedback control is performed.
In order to achieve the above object, the present invention provides a mobile robot trajectory tracking control system based on a kinematic model, which is a computer program product adapted to perform the steps of the above-mentioned mobile robot trajectory tracking control method when executed on a data processing device.
Compared with the prior art, the mobile robot track tracking control scheme based on the kinematic model has the following advantages:
(1) The method is more robust and can process some unknown disturbance or model uncertainty;
(2) The accuracy is high, the accurate tracking of a nonlinear system can be realized, and the high-accuracy control can be realized even if the dynamic state of the system is very complex;
(3) The system has high-performance control, and can realize optimization of system performance;
(4) The state measurement of the system is not required, and the cost and complexity of the system can be reduced.
Drawings
The invention is further described below with reference to the drawings and the detailed description.
FIG. 1 is a flow chart of steps of a tracking control method for a following robot in an example of the present invention;
FIG. 2 is an exemplary diagram of a kinematic model of a wheeled mobile robot in an example of the invention;
FIG. 3 is a diagram of an example pilot-follow model based on angle-distance control in an example of the invention;
FIG. 4 is a diagram illustrating an example of error relationship between a follower robot and a virtual robot in an example of the present invention;
FIG. 5 is a graph illustrating feedback of speed adjustment of a following robot in an example of the present invention;
FIG. 6 is a triangle formation retention chart of simulation experiment 1 of the present invention;
FIG. 7 is a graph of triangle formation trajectories in simulation experiment 1 of the present invention;
FIG. 8 is a graph of tracking error and speed information of a triangle formation robot in a simulation experiment 1 of the present invention;
FIG. 9 is a graph of concentric circles formed in simulation experiment 2 of the present invention;
fig. 10 is a graph of tracking error versus speed information for a circular track formation in simulation experiment 2 of the present invention.
Detailed Description
The invention is further described with reference to the following detailed drawings in order to make the technical means, the creation characteristics, the achievement of the purpose and the effect of the implementation of the invention easy to understand.
Robot trajectory tracking refers to the ability of a robot to move and track, in an automated manner, in a given environment, along a predetermined path or trajectory.
The invention provides a mobile robot track tracking control scheme based on a kinematic model.
Then, establishing a track tracking error differential equation according to the error between the actual pose and the expected pose of the mobile robot; and finally, constructing a robot track tracking controller according to a track tracking error differential equation so as to realize track tracking of the incomplete wheel type mobile robot.
Referring to fig. 1, a flow chart of the implementation of the mobile robot trajectory tracking control based on the kinematic model according to the present invention is shown.
As can be seen from the figure, the mobile robot track tracking control method based on the kinematic model provided by the invention mainly comprises the following steps:
step 1): distinguishing the relationship between the center of gravity and the shape center of the robot, and establishing a robot kinematic model;
step 2) based on a kinematic model of the robot, establishing a robot tracking error model according to the error between the actual pose and the expected pose of the mobile robot, and calculating a robot track tracking error differential equation;
step 3) generating a robot speed controller according to the error differential equation obtained in the step 2).
In the track tracking control scheme of the mobile robot, a kinematic model is established based on the relationship between the gravity center and the body center, the geometric shape, the mass distribution and the multi-joint movement of the robot are comprehensively considered, and the track tracking control scheme can be used for realizing accurate motion control.
On the basis, a robot tracking error model is further established, and accordingly, the robot is assisted to achieve accurate tracking between the actual pose and the expected pose.
By building a robot tracking error model, the control error of the robot is effectively linked to the differences between the desired poses, which can break down the control problem into smaller, easily handled parts and is typically more accurate near steady state.
Finally, based on the established robot tracking error model, a robot track tracking error differential equation is calculated, and a robot speed controller is generated according to the robot track tracking error differential equation, so that feedback control is realized, and the motion of the robot is adjusted in real time to reduce the error and realize tracking by taking the error between the actual pose and the expected pose as a feedback signal, so that the robot tracking system is suitable for different environments and changing conditions.
The following specifically describes the implementation process of the mobile robot track tracking control method based on the kinematic model.
Aiming at the step 1) in the scheme of the invention, when the robot kinematic model is established, the center of gravity and the body center of the robot are firstly and respectively distinguished and determined, the relative relation between the center of gravity and the body center of the robot is further determined according to the center of gravity and the body center of the robot, and then the robot kinematic model is established based on the relative relation between the center of gravity and the body center of the robot.
The center of gravity is specifically the average position of the whole mass distribution of the robot, and the center of gravity position is the position where the mass of the robot is concentrated, and meanwhile, the center of gravity position is fixed for the robot and cannot change with time. The determination of the position of the center of gravity is very important for the balance and stability of the robot, especially when moving and performing tasks. By way of example, the solution of the invention can be represented by a point in three-dimensional space when a kinematic model is established for the robot.
The body center here is in particular the geometric center of the robot, i.e. the midpoint of the geometry of the robot. The body center is determined without considering the mass distribution, only considering the outer contour of the robot.
In the scheme of the invention, the geometric shape, the mass distribution and the multi-joint movement of the robot are comprehensively considered through the kinematic model established based on the relation between the gravity center and the shape center of the robot, and the robot can be used for realizing accurate movement control.
Further, the scheme of the invention establishes a robot kinematic model based on the dynamic relationship between the center of gravity and the body center of the robot through the following steps:
(1) Robot geometric modeling:
first, the geometry of the robot is modeled. Specifically including taking into account the geometry and dimensions of the various joints, links and end effectors of the robot, thereby obtaining the external shape of the robot.
(2) Modeling of mass distribution:
before the kinematic model of the robot is built, the mass distribution of the robot is determined, including in particular the mass of each link, and their centroid positions, and the robot mass distribution modeling is performed accordingly.
(3) Center of gravity position determination:
based on the data modeled for the mass distribution of the robot of step (2), the center of gravity position of the entire robot system is calculated and determined. By way of example, this can be achieved by a weighted average of the centroid position of each link, wherein the weights are the quality of the links, thereby ensuring the correctness of the center position determination.
(4) Calculating the shape center:
the geometric center for the robot is obtained by a geometric modeling tool. The geometric center here is the midpoint of the outer shape of the robot, irrespective of the mass distribution.
(5) Establishing a kinematic model:
based on the determined geometrical and mass distribution information of the robot, a kinematic model of the robot may be established. The specific kinematic model specifically comprises key parameters describing the motion relation between the joints of the robot, the pose of the end effector and the robot.
(6) Distinguishing center of gravity from body center:
the relationship between the center of gravity and the center of the body is distinguished by calculating the relative position between them. The centre of gravity will not normally coincide exactly with the centre of the body, since it takes into account the mass distribution, whereas the centre of the body is simply the midpoint of the geometry.
In the embodiment, the geometric and mass distribution models of the robot are built, the center of gravity and the shape center are calculated, and the kinematic model of the robot is built so as to be used in the control and planning of the robot; meanwhile, the relationship between the center of gravity and the center of the body is effectively distinguished, and important information is provided for the motion control of the robot.
Preferably, the scheme of the invention obtains the kinematic model of the incomplete wheeled robot according to the motion characteristics of the incomplete wheeled robot, and directly inscribes the mathematical relationship between the pose and the speed of the system through the kinematic model of the incomplete mobile robot.
In particular, reference is made to fig. 1, which shows an example of a kinematic model of a wheeled mobile robot in an example of the invention.
Based on the illustration, the wheeled mobile robot is composed of two driving wheels and two steering wheels, wherein the steering wheels do not participate in the driving. Wherein C is the geometry of a connecting shaft of two wheels of the robotThe center is 2R is the distance between two driving wheels of the robot, u is the radius of the driving wheels of the robot, M is the mass center of the robot, v represents the linear velocity of the trolley, ω represents the angular velocity of the trolley, p is the distance between the mass center and the geometric center of the trolley, θ represents the included angle formed by the linear velocity direction of the trolley and the positive X-axis direction of the global coordinate system, v L And v R Representing the linear speeds of the left and right driving wheels of the trolley, respectively.
On the basis, the non-complete constraint of the robot is established because the sliding phenomenon does not occur between the wheels of the robot and the ground in the movement process of the robot, so that the movement model of the robot can be described more accurately.
Accordingly, the incomplete constraint of the mobile robot is established in the scheme of the invention as shown in the formula (1-1).
Further, in the scheme of the invention, the following mobile robot kinematic model is established:
in the established mobile robot kinematic model, the mass distribution data of the robot is utilized to calculate the gravity center position of the robot, so that the average position of the mass center of the robot can be accurately determined. Meanwhile, the geometric center of the robot body is obtained through the geometric modeling tool, and the geometric center of the external shape of the robot is accurately determined.
On the basis, the distance between the center of gravity and the center of the body is calculated, so that the kinematic model of the mobile robot can be accurately established, and the follow-up accuracy of kinematic control of the mobile robot can be ensured.
Aiming at the fact that the distance between the geometric center C and the center of mass M of the mobile robot is smaller, in the scheme, p is preferably set to be equal to 0, so that the establishment accuracy of a kinematic model of the mobile robot can be improved.
On the basis of the constructed mobile robot kinematics model, a robot tracking error model is further established, and a robot track tracking error differential equation is determined.
In order to further explain the present embodiment, the pose of each robot will be described here.
In the two-dimensional plane, the pose of each robot can be uniquely determined by three parameters under a unified global coordinate system, namely, the coordinates (x, y) in the two-dimensional plane and the yaw angle theta of the robot.
Referring to fig. 3, an example of a pilot-follow model based on angle-distance control in the present invention is shown.
The pilot-following model based on angle-distance control in the scheme of the invention is preferably established by the following steps:
1. defining a coordinate system:
first, a global coordinate system is determined for describing the position and orientation of the robot. By way of example, the global coordinate system determined in this step may be a map coordinate system or other coordinate system suitable for the application.
2. Path planning for the pilot:
the navigator robot needs to plan its path of motion to determine the trajectory or path to follow. This may be implemented using path planning algorithms (e.g., a-algorithm, dijkstra algorithm, etc.), for example. The pilot may also use sensors or visual information to perceive the surrounding environment to avoid obstacles or to adapt to dynamic environments.
3. Control target of follower:
the follower robot needs to set its control target according to the position and the movement direction of the pilot. As an example, this can be achieved by calculating the angle and distance differences between the pilot and the follower.
4. Angle-distance control:
the follower uses angle and distance control to adjust his own motion to ensure that it is maintaining the desired direction and distance from the pilot. The angle control here mainly involves controlling the direction or heading angle of the follower so as to be directed toward the pilot. The distance control is responsible for adjusting the distance between the follower and the pilot.
5. And (3) constructing a control algorithm:
a control algorithm is constructed to achieve angle and distance control of the follower robot. This may include, for example, a PID controller, model Predictive Control (MPC), or other suitable control strategy. Further, the control algorithm constructed herein requires the generation of control inputs based on the angle error and the distance error to adjust the follower's motion.
6. Real-time feedback:
the follower continuously measures the position and orientation of the pilot to calculate the angle and distance error. The real-time feedback information is used to adjust the control inputs to maintain the follower at the desired pilot position and orientation.
7. The completion follows:
the follower gradually approaches the navigator through continuous angle and distance control, so that the track or path of the navigator is followed.
Based on the pilot-following model based on the angle-distance control, it can be determined that each robot is a robot body center position as its position in the global coordinate system.
In order to quickly and accurately determine the information of the position to be reached by the following robot, a virtual robot is introduced between the following robot and the pilot robot in the scheme, and the position information of the virtual robot is used as target position information of the scheme to be controlled to reach the robot.
As such, here is determined in the global coordinate system:
pose coordinate R of pilot robot L ＝(x L ,y L ,θ L ) T ，
Pose coordinate R of virtual robot V ＝(x V ,y V ,θ V ) T ，
Following robot R F Pose coordinates R of position F ＝(x F ,y F ,θ F ) T 。
Parallel baseDetermining a following robot R by pilot-following method F And pilot robot R L Is L d And a desired relative angle of
Following robot R in global coordinate system F And pilot robot R L The actual distance and angle of (2) are L respectively c Andand has->
Wherein, (x) L ,y L ,θ L ) T ,x L ,y L Respectively representing the coordinates of the piloting robot in the x axis and the y axis in the global coordinate system, theta L Is the yaw angle of the robot;
(x V ,y V ,θ V ) T ,x V ,y V respectively representing the coordinates of the virtual robot in the x axis and the y axis in the global coordinate system, theta V Is the yaw angle of the robot;
(x F ,y F ,θ F ) T ,x F ,y F respectively representing the coordinates of the following robot in the x-axis and the y-axis in the global coordinate system, theta F Is the yaw angle of the robot.
According to the scheme, the virtual robot is innovatively introduced, the error following model between the following robot and the virtual robot is built, and the complexity of building the dynamic error model is further simplified through the relation between the following robot and the virtual robot.
In the scheme of the invention, the included angle between the advancing direction of the robot and the positive direction of the coordinate axis X is taken as the yaw angle of the robot, so that theta is calculated according to the relation between the geometric relation and the expected pose V ＝θ L 。
According to FIG. 3, a robot R is piloted in a global coordinate system L For reference coordinates, canDetermining a piloting robot R L And virtual robot R V The geometrical relationship between them is:
similarly, the robot R is piloted under a global coordinate system L For reference coordinates, the following robot R can be determined F Is the actual position of (3):
on the basis, in order to ensure the effectiveness of the subsequently constructed controller, the piloting following model in the scheme of the invention preferably realizes the piloting robot R L And a follower robot R F The distance and angle therebetween satisfy the following equation:
lim t→∞ (L d -L c )＝0 (1-5)；
on the basis of the scheme, in order to clearly and accurately determine the error between the following robot and the ideal pose, the scheme of the invention further introduces an accessory coordinate system, so that the reference robot is changed based on the introduced accessory coordinate system, and the reference robot is determined as the following robot, thereby realizing the accurate determination of the error between the following robot and the ideal pose.
Referring to FIG. 4, in the world coordinate system oxy, coordinate system o F x F y F For the coordinate system established to follow the position of the robot, x is used respectively e ,y e ,θ e Representing following robot R in an appendage coordinate system F Robot R with expected pose V At x F Axis, y F Distance errors and angle errors in the axial and azimuthal directions.
The coordinate system of the appendage introduced here and the corresponding coordinate system are under the same global coordinate system, so that coordinate rotation change and translational change can be effectively carried out between different reference coordinate systems.
Meanwhile, the tracking error deduction calculation step procedure can be further simplified based on the inclusion of the appendage coordinate system.
On the basis, the tracking robot coordinate system R F Performing rotation coordinate transformation, and obtaining a dynamic error model under a following robot coordinate system based on the incomplete constraint vertical (1-1) and the kinematic model (1-2) of the mobile robot, wherein the dynamic error model is as follows:
wherein,is a rotation transformation matrix;
(x V -x F ,y V -y F ,θ V -θ F ) T for following robot R under coordinate system oxy in FIG. 4 F And virtual robot R V Pose error of (a).
The scheme of the invention further derives an error differential equation of the determination system aiming at the established dynamic error model (1-7) under the following robot coordinate system.
The system is an error differential system deduced according to the established track tracking model; meanwhile, the relation between the track tracking model and the error differentiating system is as follows: the track tracking model obtains an error differential system by obtaining a first derivative.
Wherein,
by deriving the dynamic error model, it is here effectively achieved that the error model is changed to a kinematic model with respect to the error, wherein the derivative of the distance error is the speed.
On the basis, v of the pose robot is expected V ＝v L ，ω V ＝ω L The error differential equation thus formed is:
further, the solution of the invention solves the error differential equation by selecting a proper solving method (Backstepping method) aiming at the obtained error differential equation, so as to obtain the speed controller of the robot.
The pose error differential equation (1-11) based on the robot track tracking formed by the scheme is a nonlinear system, and for this purpose, the formed error differential equation (1-11) is calculated by adopting a back-step solution mode innovatively to obtain the final robot speed controller.
Specifically, it is first preferable to stabilize x based on the dynamic error differential equation (1-11) of the robot model e Selection control input:
v F ＝k 1 x e +v L (cosθ e -k θ θ e sinθ e ) (1-12)
wherein v is F Represents the linear velocity, x of the following robot e To follow the robot as a reference frame, errors in the x-axis, y e To follow the machineHuman reference coordinate system, error in y-axis, θ e For following the robot as a reference frame, the error of the yaw angle.
Combining the foregoing formulas (1-8) to obtain:
further, the Lyapunov function is selected asDeriving and obtaining
If x is satisfied e =0 holds, obviouslyX is true e Is asymptotically stable in the sense of Lyapunov, however x e Typically not equal to 0, and therefore requires continued design of the system.
Further, for y e Selecting Lyapunov function as
Deriving (1-15)
On the basis, the angular speed control input is further designed
ω F ＝ω L +v L (k 2 (y e +k θ θ e x e ))+k 3 sinθ e (1-17)
Wherein omega F Representative ofThe angular velocity of the following robot is determined based on how this is satisfiedA fit of the appropriate data is required. />
Further, the formula (1-16) is carried into (1-17), and there may be
Thereby meeting the following requirementsThe constant is established; further according to the Lyapunov stability criterion, the system is asymptotically stable in the sense of Lyapunov, and accordingly the effectiveness of the controller design can be effectively described.
As a further explanation, the invention provides a method for setting k in the controller based on the scheme 1 、k 2 、k 3 、k θ Is set to be greater than 0, whereby it can be determined,the constant is established; further according to the lyapunov stability criterion, the system is asymptotically stable in the sense of lyapunov.
In summary, the controller designed in the scheme of the invention is as follows:
wherein v is F To follow the robot R F Linear velocity, omega F To follow the robot R F Is a function of the angular velocity of the rotor.
So far, in the multi-robot formation, the following robot R F And (5) calculating the linear velocity and the angular velocity.
By further explanation, the scheme of the invention is based on the determined speed controller of the robot, and the gesture of the following robot can be adjusted according to the speed feedback of the following robot, so that the following robot can track the pilot robot accurately.
As an example, as shown in fig. 5, when performing trajectory tracking control, the following process of adjusting the pose of the following robot according to the following robot speed feedback is as follows:
1. modeling robot geometry and quality:
first, a geometric and mass distribution model of the robot is built, including the geometric shape, mass and centroid position of each link, specifically to calculate the center of gravity position and the body center of the robot.
2. Center of gravity and shape center calculation:
the center of gravity position of the robot is calculated using the mass distribution data of the robot, which is the average position of the center of mass of the robot. Meanwhile, the body center of the robot, i.e., the geometric center of the external shape of the robot, is obtained through a geometric modeling tool.
3. Establishing a kinematic model:
and establishing a kinematic model of the robot by utilizing the geometric and mass distribution information, and describing the motion relation between the joints of the robot, the pose of the end effector and key parameters.
4. Calculating a motion error:
when the robot starts to perform a task, the actual speed and position are measured, and an error from the target speed is calculated, thereby serving as a basis for feedback control.
5. Designing a speed controller:
based on the speed error, a speed controller is constructed, here using a proportional-integral-derivative (PID) controller or other control strategy. The speed controller generates control inputs to adjust the speed of the robot to reduce speed errors.
6. Feedback speed control:
the output of the speed controller is implemented to adjust the joint speed and end effector speed of the robot to trend the robot toward the target speed.
7. Real-time feedback:
the actual speed and position are measured continuously, the speed error is recalculated, and the output of the speed controller is adjusted according to the error, so that continuous feedback control is realized.
8. And (3) correcting in real time:
if the relative position between the center of gravity and the center of the body of the robot changes during the movement, the speed controller corrects according to the real-time situation to maintain the stability of the robot.
9. The task is completed:
the robot gradually approaches to the target speed through continuous speed control and feedback, and finally the task is completed.
According to the scheme, the geometric and mass distribution model of the robot is built, the center of gravity and the shape center are calculated, and then the robot can be adjusted along with the target speed by combining speed control and feedback control, so that high-precision execution of tasks is realized. The design and feedback mechanism of the speed controller in the scheme ensures that the robot can maintain stability and accuracy in a continuously changing environment.
Aiming at the mobile robot track tracking control scheme based on the kinematic model, which is provided by the invention, when the mobile robot track tracking control scheme is applied specifically, a corresponding software program can be formed, and a corresponding mobile robot track tracking control system is formed. When the software program runs, the mobile robot track tracking control method is executed, and meanwhile, the software program is stored in a corresponding storage medium for being called and executed by a processor.
In order to further explain the effectiveness and technical characteristics of the mobile robot track tracking control scheme based on the kinematic model, the invention further combines an ROS robot operating system and a simulation environment Gazebo, adopts two formations of a pilot robot of a following robot to form a corresponding simulation example, and further verifies the rationality, effectiveness and superiority of the mobile robot track tracking control scheme based on the kinematic model.
Specifically, the verification example specifically combines simulation experiments on a three-dimensional simulation software Gazebo platform. The software can simulate robots in real environments, and the control interface of the software is identical to the trolley control interface developed by using ROS, so that the verified algorithm can be deployed on the real ROS robots.
Simulation experiment 1: triangle formation
In the simulation experiment, three robots are formed into a triangle from an initial position after pose adjustment and then go forward along a straight line, wherein a robot1 is set as a pilot robot, and a robot2 and a robot3 are set as following robots. The pose of the robot at the initial time is shown in table 1-1.
TABLE 1-1 parameters at the initial time of triangle formation
Wherein, the piloting robot1 moves at a constant speed along the straight line y=0 at the linear speed v=0.3 m/s and the angular speed ω=0. The following robot tracks a virtual pilot under the action of the controllers (1-19), and the expected relative distance l=1m is set in the experiment, wherein the expected relative angles of the robot2 and the robot3 are respectivelySimulation results show that both control methods A and B can form the formation rapidly and keep the formation stable. Referring to fig. 6, a triangle formation holding diagram in the simulation experiment is shown, and as shown, the trolley formation is stable in the simulation environment.
Further, the motion data and sensor parameters of the pilot robot and the follower robot can be recorded by a rosbag tool of the ROS, and the monitored data information is plotted by a toolkit PlotJuggler software of the ROS.
The motion trail of the triangle formation robot obtained in the simulation platform Gazebo according to the table 1-1 is shown in fig. 7, and the content shown in fig. 7 shows that the tracking error between the following robot and the ideal pose can be effectively controlled based on the scheme of the invention.
Further, following the robot2, robot3 position errors is shown in fig. 8 a), robot yaw angle is shown in fig. 8 b), robot linear velocity is shown in fig. 8 c), and robot angular velocity is shown in fig. 8 d).
Based on the content shown in fig. 8, the scheme of the invention can achieve the following indexes and has good control effect:
lim t→∞ (L d -L c )＝0 (1-5)；
simulation experiment 2: round track formation
In the simulation experiment, the tracks of all robots formed under the formation of the circular track are circles with a certain radius, and the initial pose and the initial speed of the formation of the robots are set as shown in tables 1-2.
Table 1-2 parameters of round trajectory formation initiation time
According to tables 1-2, the pilot robot1 performs a uniform circular motion with a linear velocity v=0.3 m/s and an angular velocity ω=0.1 rad/s. Robot2, robot3 moves according to corresponding linear velocity and angular velocity under the action of formation controller, the experiment sets that the expected distance l=1m between the follower and the pilot is expected to keep, wherein the expected angle of robot2 isrobot3 desired angle->The motion trajectory of the robot in the circular trajectory thus obtained is shown in fig. 9. As can be seen from the illustration in fig. 9, the tracking error between the following robot and the ideal pose can be effectively controlled based on the scheme of the invention.
Further, the obtained concentric circle track follows the robot position error as shown in fig. 10 a), the robot yaw angle as shown in fig. 10 b), the robot linear velocity as shown in fig. 10 c), and the robot angular velocity as shown in fig. 10 d).
Based on the content shown in fig. 10, the scheme of the invention can achieve the following indexes and has good control effect:
lim t→∞ (L d -L c )＝0 (1-5)；
compared with the existing model prediction combined with synovial membrane control scheme, the track tracking control scheme formed based on the scheme has the following advantages:
1. smoothness:
the controller designed by the scheme can provide smooth control input, because the abrupt change of the input speed in the movement process of the trolley can cause larger acceleration, and the motor performance and control of the trolley have higher performance requirements. The control input of the slipform controller typically causes high frequency switching, which may result in less than ideal control of the system.
2. Robustness:
the controller designed by the scheme has better robustness in a nonlinear system, and can cope with some unmodeled dynamic characteristics and external disturbance. The design of the back-stepping method allows for stable control in the event that the system model is not completely accurate or there is uncertainty.
3. Recursion:
the controller designed according to the scheme designs the controller in a recursive manner, and gradually advances the system state to evolve to the expected track. This recursive nature makes the back-stepping method suitable for a range of nonlinear systems and facilitates repeated applications in different systems.
4. Target tracking:
the controller designed by the scheme is very powerful in target tracking and track tracking, and can effectively realize that a nonlinear system tracks a desired track without an accurate system model.
5. Little dependence on slip form face design:
the performance of a slip-form controller is typically highly dependent on the design of the slip-form surface, which may require some expertise to select. The backstepping method adopted in the scheme is generally less dependent on complex sliding mode surface design and is easier to implement.
The mobile robot track tracking control scheme provided by the invention can be applied to various fields including industrial automation, logistics, military, medical and service robots and the like when being practically applied. The mobile robot track tracking control scheme provided by the invention has the following excellent effects in practical application:
1. accuracy and reliability: the robot track tracking system can realize high precision and reliability, so that the robot can accurately move along a designated path, and errors and risks are reduced.
2. Automation and efficiency: through track tracking, the robot can automatically execute tasks, so that the need of manual intervention is reduced, and the production efficiency and the throughput of a production line are improved.
3. Safety: the track tracking system can help the robot to avoid collision with obstacles, so that the risks of accidents and damage are reduced, and the safety of a working environment is ensured.
4. Multi-domain application: the robot track tracking technology is widely applied to various fields including industrial manufacturing, agriculture, medical care, logistics, transportation and the like, and provides solutions for different industries.
5.24/7 run capability: the robot can execute track tracking tasks in an uninterrupted working period without rest, so that the continuity and usability of production are improved.
6. Data collection and analysis: trajectory tracking systems typically record robot motion data that can be used to analyze and optimize the production process, improve path planning and resource allocation.
7. The adaptability: some track tracking systems have self-adaptive capability, and can be adjusted according to environmental changes and task requirements, so that the adaptability and the flexibility of the robot are improved.
The above method of the present invention, or specific system units, or parts thereof, are pure software structures, and can be distributed on physical media, such as hard disks, optical discs, or any electronic devices (such as smart phones, computer readable storage media), when the machine loads the program codes and executes (such as smart phones loads and executes), the machine becomes a device for implementing the present invention. The methods and apparatus of the present invention may also be embodied in the form of program code that is transmitted over some transmission medium, such as over electrical wiring, optical fiber, or any other transmission medium, when the program code is received and loaded into and executed by a machine, such as a smart phone, the machine thereby providing an apparatus for practicing the methods.
The foregoing has shown and described the basic principles, principal features and advantages of the invention. It will be understood by those skilled in the art that the present invention is not limited to the embodiments described above, and that the above embodiments and descriptions are merely illustrative of the principles of the present invention, and various changes and modifications may be made without departing from the spirit and scope of the invention, which is defined in the appended claims. The scope of the invention is defined by the appended claims and equivalents thereof.
Claims (8)
1. The mobile robot track tracking control method based on the kinematic model is characterized by comprising the following steps:
1) Distinguishing the relationship between the center of gravity and the shape center of the robot, and establishing a robot kinematic model;
2) Based on a kinematic model of the robot, a tracking error model of the robot is established according to the error between the actual pose and the expected pose of the mobile robot, and a tracking error differential equation of the robot track is calculated.
3) Generating a robot speed controller according to the error differential equation obtained in the step 2).
2. The mobile robot trajectory tracking control method according to claim 1, wherein when the robot kinematic model is built in step 1), the center of gravity and the body center of the robot are first determined separately, the relative relationship between the center of gravity and the body center of the robot is further determined based on the determined differences, and the robot kinematic model is built based on the relative relationship between the center of gravity and the body center of the robot.
3. The mobile robot trajectory tracking control method according to claim 2, wherein when the robot kinematic model is built in step 1), comprising the steps of:
(1.1) robot geometric modeling:
firstly, modeling a geometric structure of a robot;
(1.2) modeling of mass distribution:
modeling mass distribution of the robot;
(1.3) center of gravity position calculation:
based on mass distribution modeling of the robot, calculating and determining the gravity center position of the whole robot system;
(1.4) calculation of body center:
geometric modeling is carried out on the robot, the geometric center point counting position of the external shape of the robot is determined, and mass distribution is not considered;
(1.5) distinguishing center of gravity from body center:
the relation between the center of gravity and the center of the body is distinguished by calculating the relative position between the center of gravity and the center of the body;
(1.6) establishing a kinematic model:
and establishing a kinematic model of the robot based on the relation between the gravity center and the shape center of the robot.
4. The mobile robot trajectory tracking control method according to claim 1, wherein the building of the robot tracking error model in step 2) includes:
(2.1) constructing a global coordinate system;
(2.2) determining in a global coordinate system: pose coordinate R of pilot robot L ＝(x L ，y L ，θ L ) T Pose coordinate R of virtual robot V ＝(x V ，y V ，θ V ) T Following robot R F Pose coordinates R of position F ＝(x F ，y F ，θ F ) T ；
(x L ，y L ，θ L ) T ，x L ，y L Respectively representing the coordinates of the piloting robot in the x axis and the y axis in the global coordinate system, theta L Is the yaw angle of the robot;
(x V ，y V ，θ V ) T ，x V ，y V respectively representing the coordinates of the virtual robot in the x axis and the y axis in the global coordinate system, theta V Is the yaw angle of the robot;
(x F ，y F ，θ F ) T ，x F ，y F respectively representing the coordinates of the following robot in the x-axis and the y-axis in the global coordinate system, theta F Is the yaw angle of the robot;
(2.3) determining the virtual robot R according to the pose coordinates determined in the step (2.2) V Relative to piloting robot R L Is a model of the actual position of:
determining a following robot R F Relative to piloting robot R L Is a model of the actual position of:
wherein L is d To follow the robot R F And pilot robot R L Is a desired relative distance of (2);
is a desired relative angle;
L c to follow the robot R in the global coordinate system F And pilot robot R L Is the reality of (2)The distance between the two points is equal to the distance between the two points,to follow the robot R in the global coordinate system F And pilot robot R L Is provided with->
(2.4) constructing a dynamic error model under a following robot coordinate system based on the robot kinematics model established in the step (1):
wherein, (x) V -x F ，y V -y F ，θ V -θ F ) T To follow the robot R in world coordinate system oxy F And virtual robot R V Pose error of (2);
(2.5) calculating and determining a robot track tracking error differential equation based on the dynamic error model constructed in the step (2.4):
5. the mobile robot trajectory tracking control method according to claim 1, wherein the robot speed controller is generated in step 3) in a back-stepping manner based on the error differential equation obtained in step 2).
6. The mobile robot trajectory tracking control method according to claim 1, characterized in that the robot speed controller formed in step 3) is as follows:
wherein v is F To follow the robot R F Linear velocity, omega F To follow the robot R F Is a function of the angular velocity of the rotor.
7. The mobile robot trajectory tracking control method according to claim 1, characterized in that the trajectory tracking control method controls a process of robot following target speed adjustment feedback based on a robot speed controller, comprising:
(3.1) calculating a motion error:
when the robot starts to execute the task, measuring the actual speed and the position, and calculating the error between the actual speed and the target speed to serve as a basis of feedback control;
(3.2) designing a speed controller:
constructing a speed controller based on the established speed controller model according to the speed error, the speed controller generating a control input to adjust the speed of the robot;
(3.3) feedback speed control:
implementing the output of the speed controller, adjusting the joint speed and the end effector speed of the robot to cause the robot to trend toward the target speed;
(3.4) real-time feedback:
the actual speed and position are measured continuously, the speed error is recalculated, and the output of the speed controller is adjusted according to the error, so that continuous feedback control is performed.
8. A mobile robot trajectory tracking control system based on a kinematic model, which is a computer program product, characterized by being adapted to perform the steps of the mobile robot trajectory tracking control method of any one of claims 1-7 when executed on a data processing device.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311473092.0A CN117724472A (en) | 2023-11-07 | 2023-11-07 | Mobile robot track tracking control method and system of kinematic model |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311473092.0A CN117724472A (en) | 2023-11-07 | 2023-11-07 | Mobile robot track tracking control method and system of kinematic model |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117724472A true CN117724472A (en) | 2024-03-19 |
Family
ID=90206067
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311473092.0A Pending CN117724472A (en) | 2023-11-07 | 2023-11-07 | Mobile robot track tracking control method and system of kinematic model |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117724472A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118409507A (en) * | 2024-06-25 | 2024-07-30 | 广州大学 | A method and system for constructing a multi-agent adaptive synchronous iterative learning coordination controller |
-
2023
- 2023-11-07 CN CN202311473092.0A patent/CN117724472A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118409507A (en) * | 2024-06-25 | 2024-07-30 | 广州大学 | A method and system for constructing a multi-agent adaptive synchronous iterative learning coordination controller |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Wen et al. | Elman fuzzy adaptive control for obstacle avoidance of mobile robots using hybrid force/position incorporation | |
| Wang et al. | A hybrid visual servo controller for robust grasping by wheeled mobile robots | |
| Zhang et al. | Point stabilization of nonholonomic mobile robot by Bézier smooth subline constraint nonlinear model predictive control | |
| CN113848905B (en) | Mobile robot track tracking method based on neural network and self-adaptive control | |
| CN113625702B (en) | Simultaneous path tracking and obstacle avoidance method for unmanned vehicles based on quadratic planning | |
| Ribeiro et al. | Nonlinear model predictive visual path following control to autonomous mobile robots | |
| CN108189034B (en) | Method for realizing continuous track of robot | |
| Tsakiris et al. | Extending visual servoing techniques to nonholonomic mobile robots | |
| CN114378830B (en) | Robot wrist joint singular avoidance method and system | |
| CN112650217B (en) | Robot trajectory tracking strategy dynamic optimization method based on evaluation function | |
| Liu et al. | An improved hybrid error control path tracking intelligent algorithm for omnidirectional AGV on ROS | |
| Xu et al. | Model predictive control-based path tracking control for automatic guided vehicles | |
| CN117724472A (en) | Mobile robot track tracking control method and system of kinematic model | |
| Quang et al. | Design a nonlinear MPC controller for autonomous mobile robot navigation system based on ROS | |
| CN114952838B (en) | A trajectory planning method for manipulator joints based on end measurement feedback | |
| Shin et al. | Explicit path tracking by autonomous vehicles | |
| Sahoo et al. | Design and development of a heading angle controller for an unmanned ground vehicle | |
| Potiris et al. | Terrain-based path planning and following for an experimental mars rover | |
| CN119126786A (en) | A method for controlling the motion of a crawler chassis | |
| Tang | Localization and tracking control for mobile welding robot | |
| CN117930661A (en) | Differential AGV control method, device, trolley and readable storage medium | |
| Patel et al. | Simulation-based analysis of highway trajectory planning using high-order polynomial for highly automated driving function | |
| Su et al. | Adaptive coordinated motion constraint control for cooperative multi-manipulator systems | |
| CN117724471A (en) | Mobile robot track tracking control method and system based on speed feedback control | |
| Kanjanawanishkul | Coordinated path following for mobile robots using a virtual structure strategy with model predictive control |
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 |