A method for dynamically tracking the motion trajectory of a mobile printing robot

Abstract

The present invention discloses a method for dynamically tracking the motion trajectory of a mobile printing robot, comprising the following steps: first, obtaining the relationship between the velocity of a particle and the actual motion trajectory curve; second, obtaining the complete and incomplete constraints of the mobile printing robot structure based on the constraint relationship between the wheel rotation speed and the motion velocity of the mobile printing robot, deriving the dynamic equations of the mobile printing robot using the Euler-Lagrange equation, and converting them into the speed target of the mobile printing robot; and third, calculating the relative curvature based on the speed target of the mobile printing robot, introducing the relative curvature to design a dynamic trajectory tracking target, and designing a controller. The present invention designs a precise and efficient trajectory tracking controller for the wheeled mobile structure, thereby accurately achieving a given motion task.

Description

Technical Field

[0001] The present invention relates to the field of motion trajectory of a mobile printing robot, and in particular to a method for dynamically tracking the motion trajectory of a mobile printing robot. Background Art

[0002] A mobile printing robot is a new type of high-precision miniature wheeled mobile robot that combines robotics and printing technologies. This mechanical model achieves printing by driving its wheels. It offers advantages such as portability, low energy consumption, and convenient printing. During the mobile printing process, the robot must follow a given trajectory and maintain a zero lateral velocity, thus satisfying a nonholonomic constraint. Existing mobile printing robots suffer from slippage and error accumulation during motion, leading to errors and defects in the printing process. Therefore, achieving high-precision position positioning and dynamic trajectory tracking for mobile printing robots is crucial to precisely control their motion. In recent years, research institutions both domestically and internationally have invested significant effort in this field, with numerous studies on modeling, path tracking, and control design, further promoting the development and advancement of mobile printing robots. However, accurate tracking requires understanding the robot's kinematic and dynamic equations. Currently, many studies focus solely on its kinematics, considering tracking given forward velocity and yaw speed targets and designing adaptive fuzzy or neural network control to achieve the tracking task. However, the geometric trajectory corresponding to the given forward velocity and yaw speed targets remains unclear. Practical problems require designing tracking controls that enable wheeled mobile structures to precisely follow a given target trajectory. Therefore, both the kinematic and dynamic equations must be considered simultaneously, and force or torque controllers designed to enable the robot to achieve the desired motion task. Furthermore, existing research lacks a deep understanding of the motion laws of wheeled nonholonomic constraints. While many studies focus on control design and analysis from a control theory perspective, few studies have effectively integrated the motion laws of wheeled nonholonomic constraints with robot motion control design. Therefore, this area holds significant research value. Summary of the Invention

[0003] Existing mobile printing robots have the disadvantages of losing step and slipping during movement, as well as error accumulation, making it very difficult to achieve accurate trajectory tracking of the mobile printing robot, resulting in the mobile printing robot being unable to accurately complete a given motion task.

[0004] The present invention provides a method for dynamically tracking the motion trajectory of a mobile printing robot. This method accurately tracks the motion trajectory curve of a mobile printing robot based on a combination of kinematic and dynamic equations, thereby achieving precise control of the mobile printing robot's motion. Specifically, the nonholonomic constraints of a wheeled mobile structure and the motion patterns of the mechanical structure under these constraints are analyzed. Then, by introducing relative curvature, a dynamic trajectory tracking target is designed for the task target curve. Based on this dynamic trajectory tracking target, the target trajectory curve, kinematic equations, and dynamic equations in a practical problem can be organically combined to design an accurate and efficient trajectory tracking controller for the wheeled mobile structure, thereby precisely achieving a given motion task.

[0005] A method for dynamically tracking the motion trajectory of a mobile printing robot comprises the following steps:

[0006] Step 1: Analyze the motion characteristics of the particle along the smooth curve, obtain the relationship between the particle's motion speed and the actual motion trajectory curve, and obtain the motion law of the mobile printing robot;

[0007] Step 2: Based on the constraint relationship between the wheel speed and the movement speed of the mobile printing robot, the holonomic and nonholonomic constraints of the mobile printing robot structure are obtained. The dynamic equation of the mobile printing robot is derived by combining the holonomic and nonholonomic constraints of the mobile printing robot structure using the Euler-Lagrange equation. The target trajectory curve of the mobile printing robot is converted into the speed target of the mobile printing robot according to the dynamic equation.

[0008] Step 3: Calculate the relative curvature based on the speed target of the mobile printing robot, introduce the relative curvature to design the dynamic trajectory tracking target, and combine the motion law and dynamic equation of the mobile printing robot through the dynamic trajectory tracking target to design a controller.

[0009] In step 1, the characteristics of the particle motion along the smooth curve are analyzed, specifically:

[0010]

[0011] Among them, θ is the angle between the tangent vector of the printing robot motion curve and the x-axis, are the first derivatives of displacement in the x and y directions, respectively.

[0012] In step 1, the relationship between the velocity of the particle and the actual motion trajectory curve is as follows:

[0013] Given a smooth curve The curve is regarded as the motion trajectory of a particle. By taking the derivative of t, the relationship between the velocity of the particle and the motion trajectory curve is as follows:

[0014]

[0015] in, is the tangent vector length of the curve, and its physical meaning is the forward velocity of the particle motion. is the state estimate of the angle between the tangent vector of the motion curve and the x-axis, is the state estimate of the yaw speed of the particle motion, is the state estimate of the displacement in the x direction, is the state estimate of the displacement in the y direction, is the first derivative of the state estimate of the displacement in the x direction, is the first derivative of the state estimate of the displacement in the y direction, is the second derivative of the state estimate of the displacement in the x direction, is the second-order derivative of the state estimate of the displacement in the y direction, represents the product of the first derivative of the state estimate of the displacement in the x-direction and the second derivative of the state estimate of the displacement in the y-direction, Represents the product of the second derivative of the state estimate of the displacement in the x-direction and the first derivative of the state estimate of the displacement in the y-direction.

[0016] In step 2, the dynamic equation of the mobile printing robot is specifically:

[0017]

[0018] in:

[0019]

[0020]

[0021]

[0022]

[0023] It is controlled by the angle between the front wheel direction and the middle pole of the mobile printing robot. As a control variable, is the front wheel speed of the mobile printing robot, q is the state vector of the mobile printing robot, is the derivative of the state vector of the mobile printing robot, x is the displacement in the x direction, y is the displacement in the y direction, θ is the angle between the tangent vector of the motion curve and the x axis, θ l is the angle between the left wheel and the x-axis, θ r is the angle between the right wheel and the x-axis, v is the forward velocity of the particle, is the yaw speed of the printing robot, d is the distance between the left and right wheels, a is the length of the middle rod, r is the wheel radius, V is the velocity matrix, is the derivative of the velocity matrix, S is the constraint matrix of the mobile printing robot for the direction of q value, is the derivative of the constraint matrix of the mobile printing robot for the direction of q value, S Τ is the transposed matrix of the constraint matrix of the mobile printing robot for the direction of q value, A is the inertia matrix of the mobile printing robot, represents the centripetal force and Golgi force matrix of the system, T is the input torque vector of the mobile printing robot, E(q) is the matching matrix, T l ,T r They are the torques provided by the left and right wheels respectively, M1 represents the parameters related to the wheel mass radius and moment of inertia, I ω represents the moment of inertia of the wheel around the wheel axis, and I represents the moment of inertia of the mobile printing robot.

[0024] In step 2, the target trajectory curve of the mobile printing robot is converted into the speed target of the mobile printing robot according to the dynamic equation, which specifically includes:

[0025] The dynamic equation is converted into the state equation of the mobile printing robot:

[0026]

[0027] in:

[0028]

[0029]

[0030] is the angle between the front wheel direction of the mobile printing robot and the middle pole. As a control variable, is the front wheel speed of the mobile printing robot, θ is the angle between the tangent vector of the motion curve and the x-axis, is the second derivative of the angle between the tangent vector of the motion curve and the x-axis, v is the forward velocity of the particle motion, is the first derivative of the forward velocity of the particle motion, d is the distance between the left and right wheels, a is the length of the middle rod, r is the wheel radius, T l ,T r They are the torques provided by the left and right wheels, c is the distance d between the two wheels, the wheel radius r, and the moment of inertia of the wheel around the wheel axis I ω Related parameters, M1 represents the parameters related to the wheel mass radius and moment of inertia, Δ is the change, I ωrepresents the moment of inertia of the wheel around the wheel axis, I represents the moment of inertia of the mobile printing robot, u1 is the intermediate control variable of the tracking controller, u2 and Determines the two variables of the tracking controller.

[0031] Using the formula (1.9) for the relationship between the velocity and trajectory of a particle, the target trajectory curve is converted into velocity form.

[0032] In step 3, the relative curvature is calculated based on the speed target of the mobile printing robot, specifically:

[0033] For a given target trajectory curve The relationship between the velocity and trajectory of the particle is used to convert it into a velocity target:

[0034]

[0035] in is the relative curvature of the target curve

[0036] is the tangent vector length of the curve, and its physical meaning is the forward speed of the printing robot. is the state estimation value of the angle between the tangent vector of the printing robot motion curve and the x-axis, is the state estimate of the yaw speed of the printing robot, is the first-order derivative of the state estimate of the x-direction displacement of the printing robot, is the first-order derivative of the state estimate of the printing robot’s y-direction displacement, is the second-order derivative of the state estimate of the x-direction displacement of the printing robot, is the second-order derivative of the state estimate of the printing robot’s y-direction displacement, Represents the product of the first-order derivative of the state estimation value of the printing robot's x-direction displacement and the second-order derivative of the state estimation value of the y-direction displacement, Represents the product of the second derivative of the print robot's x-direction displacement state estimate and the first derivative of the y-direction displacement state estimate. s(t) is the print robot's trajectory curve integrated from time 0 to time t, and k(s(t)) is the relative curvature of the print robot's trajectory curve.

[0037] In step 3, relative curvature is introduced to design a dynamic trajectory to track the target, including:

[0038]

[0039] is the forward speed of the printing robot, is the state estimation value of the angle between the tangent vector of the printing robot motion curve and the x-axis, is the state estimate of the yaw speed of the printing robot, φ is the corresponding smooth function, s(t) is the trajectory curve obtained by integrating the printing robot from time 0 to time t, and k(t) is the relative curvature of the printing robot trajectory curve.

[0040] Design according to actual needs, v(t) is the actual forward speed;

[0041] φ(t) is designed to be

[0042] φ(t)=-(βt+1)le- βt +l

[0043] l is the length of the target trajectory curve, β is an adjustable parameter, φ(t) is the corresponding smooth function, and e is an irrational number.

[0044]

[0045] l is the length of the target trajectory curve, β is an adjustable parameter, is the first derivative of the corresponding smooth function, and e is an irrational number.

[0046] Dynamic target tracking is implemented as follows:

[0047] The target trajectory curve is converted into a velocity form and combined with the state equation of the mobile printing robot to realize trajectory tracking control. u1 is the intermediate control variable and the state equation is decoupled. First, u1 is designed according to the state equation, and then combined with the nonholonomic constraint equation to obtain:

[0048]

[0049] is the yaw velocity of the printing robot, v is the forward velocity of the particle motion, is the angle between the front wheel direction of the mobile printing robot and the middle rod, a is the length of the middle rod, u1 is the intermediate variable of the controller, c is the distance d between the two wheels, the wheel radius r, and the moment of inertia of the wheel around the wheel axis I ω Related parameters.

[0050] Equation (2.15) gives the control variables u1 and The relationship between the control variables Substitute into equation (2.14), design the control variable u2, complete the controller design, and achieve the final trajectory tracking control task.

[0051] Compared with the prior art, the present invention has the following advantages:

[0052] (1) First, we analyze the characteristics of a particle moving along a smooth curve. When a particle moves along a smooth curve, the lateral velocity must be zero. Similarly, different wheeled mobile structures must satisfy this nonholonomic constraint when performing smooth curve motion on a plane.

[0053] (2) In order to achieve real-time high-precision positioning of the mobile printing robot, the present invention combines the complete constraint relationship between the wheel rotation speed and their movement speed to obtain the complete and incomplete constraints of the mobile printing robot structure, and then uses the Euler-Lagrange equation to derive the dynamic equation of the mobile printing robot.

[0054] In the present invention, the dynamic equation obtained by using the Euler-Lagrange equation for modeling is about speed, the target trajectory curve is converted into a speed target, and the general trajectory tracking control problem is obtained.

[0055] (3) In addition, based on the non-holonomic constraint, the target trajectory curve is converted into the form of a speed target, and then the relative curvature is introduced to design a dynamic tracking target. The motion law and dynamic equation of the mobile printing robot are combined through the dynamic trajectory tracking target, thereby realizing the accurate trajectory tracking of the mobile printing robot.

[0056] (4) The present invention combines the kinematic equations and dynamic equations of the robot, and proposes a method for accurately tracking the motion trajectory curve of the mobile printing robot, thereby achieving accurate control of the motion of the mobile printing robot. At the same time, the yaw speed target in the present invention is adjusted in real time according to the actual forward speed, which can greatly reduce the cumulative position error. Even if the forward speed error is very large or even the error system is unstable, as long as the yaw speed target can be accurately tracked, it can be ensured that the unicycle moves along the given target trajectory curve. The tracking effect can also be optimized by designing a suitable forward speed target.

[0057] (5) The dynamic target tracking method proposed in the present invention fundamentally solves the problem of accurate tracking of trajectory curves. Any other advanced control design method cannot achieve such accurate results from the perspective of control design alone. In fact, the dynamic target tracking method can be used to solve the precise motion control problems of various types of mechanical structures subject to this non-holonomic constraint. Even any control problem that requires accurate tracking of a given smooth trajectory curve can use this method to achieve the purpose of accurate tracking. Furthermore, subsequent work can also introduce torsion and extend the dynamic target tracking method to the case of spatial trajectory curves, which will greatly expand the scope of application of the dynamic target tracking idea. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 It is a simplified diagram of the motion of a charged particle in a plane electric field;

[0059] Figure 2 A simple diagram of the mobile printing robot;

[0060] Figure 3 It is the velocity diagram of point P;

[0061] Figure 4 Printing robot's forward velocity error;

[0062] Figure 5 Printing robot's forward velocity error;

[0063] Figure 6 Print the actual motion trajectory of the robot on the plane. DETAILED DESCRIPTION

[0064] The technical solution of the present invention is:

[0065] (1) First, we analyze the characteristics of a particle moving along a smooth curve. When a particle moves along a smooth curve, the lateral velocity must be zero. Similarly, different wheeled mobile structures must satisfy this nonholonomic constraint when performing smooth curve motion on a plane.

[0066] (2) In order to achieve real-time high-precision positioning of the mobile printing robot, the present invention combines the complete constraint relationship between the wheel rotation speed and their movement speed to obtain the complete and incomplete constraints of the mobile printing robot structure, and then uses the Euler-Lagrange equation to derive the dynamic equation of the mobile printing robot.

[0067] In the present invention, the dynamic equation obtained by using the Euler-Lagrange equation for modeling is about speed, the target trajectory curve is converted into a speed target, and a general trajectory tracking control problem is obtained.

[0068] (3) In addition, based on the non-holonomic constraint, the target trajectory curve is converted into the form of a speed target, and then the relative curvature is introduced to design a dynamic tracking target. The motion law and dynamic equation of the mobile printing robot are combined through the dynamic trajectory tracking target, thereby realizing the accurate trajectory tracking of the mobile printing robot.

[0069] The specific steps for analyzing the motion characteristics of a particle along a smooth curve described in step (1) above are as follows:

[0070] (1a) The conditions for the continuous smooth motion of a particle are as follows:

[0071] like Figure 1As shown in the figure, a charged particle with mass m moves in a smooth curve on a plane under the action of an electric field. If we want to make the particle move along a given smooth curve, how can we control the electric field force to accurately achieve the motion task? This is a simple trajectory tracking control problem. The particle does not seem to be subject to any constraints. We can directly use Newton's second law to obtain its dynamic equation.

[0072]

[0073] Where: T x ,T y are the electric field control forces along the coordinate directions respectively. are the second-order derivatives of displacement in the x and y directions, respectively, and m is the mass of the charged particle.

[0074] Assume that a given smooth curve is a track groove on a plane, and a particle moves in this curve groove. Then, is the particle subject to a constraint? The answer is yes. Is the constraint a complete constraint or a non-complete constraint? Note that when the particle moves along the curve, its velocity in the transverse direction perpendicular to the tangent is 0, that is,

[0075]

[0076] Where θ is the angle between the tangent vector of the motion curve and the x-axis, are the first derivatives of displacement in the x and y directions, respectively. This can be understood as the yaw rate of motion of a particle. In reality, when a particle moves in a smooth curved groove, it is constrained by the groove, causing its velocity to satisfy the constraint equation (1.2). Therefore, the particle is subject to the nonholonomic constraint of the groove. However, if there were no track groove, the velocity of the particle moving in a smooth curved motion on a plane must also satisfy equation (1.2). If the particle suddenly experiences a non-zero lateral velocity at a certain moment, this will result in a sharp point in its trajectory, rather than a smooth curve.

[0077] Assume that the coordinates of the particle in plane motion are represented by a smooth curve r = (x(t), y(t)), and let the length of the tangent vector of the motion trajectory be

[0078]

[0079] v is the moving speed of the car, are the first derivatives of displacement in the x and y directions, respectively.

[0080] Combining (1.2) and (1.3), we can get

[0081]

[0082] v is the moving speed of the car, are the first-order derivatives of displacement in the x and y directions, respectively, and θ is the angle between the tangent vector of the motion curve and the x-axis.

[0083] Equation (1.4) shows the main characteristics of the particle moving along a smooth trajectory curve: the lateral velocity perpendicular to the tangent is zero, and the tangent vector of the trajectory curve is the forward velocity of the particle moving along the curve. Equation (1.4) can actually be equivalent to

[0084]

[0085] v is the moving speed of the car, are the first-order derivatives of displacement in the x and y directions, respectively, and θ is the angle between the tangent vector of the motion curve and the x-axis.

[0086] Equation (1.2) is a necessary condition for the particle's trajectory to be a smooth curve. When the particle's motion satisfies Equation (1.4), the relationship between its forward velocity and yaw rate of rotation and the actual trajectory curve can be deduced.

[0087] (1b) The relationship between the velocity of a particle and its actual trajectory is as follows:

[0088] Given a smooth curve If the curve is regarded as the motion trajectory of a particle, then the velocity component of the particle along the coordinate direction is

[0089]

[0090] in: is the state estimate of the forward velocity of the particle motion, is the state estimate of the angle between the tangent vector of the motion curve and the x-axis, is the first derivative of the state estimate of the displacement in the x direction, is the first derivative of the state estimate of the displacement in the y direction

[0091] Derivative of (1.6) with respect to t yields

[0092]

[0093] Where v is the forward velocity of the particle, is the state estimate of the forward velocity of the particle motion, is the state estimate of the angle between the tangent vector of the motion curve and the x-axis, is the state estimate of the yaw speed of the particle motion, is the first derivative of the state estimate of the displacement in the x direction, is the first derivative of the state estimate of the displacement in the y direction, is the second derivative of the state estimate of the displacement in the x direction, is the second derivative of the state estimate of the displacement in the y direction, represents the product of the first derivative of the state estimate of the displacement in the x-direction and the second derivative of the state estimate of the displacement in the x-direction, Represents the product of the first derivative of the state estimate of the displacement in the y direction and the second derivative of the state estimate of the displacement in the y direction.

[0094] Cross-multiply (1.6) and (1.7) and subtract them to get

[0095]

[0096] is the state estimate of the yaw speed of the particle motion, is the first derivative of the state estimate of the displacement in the x direction, is the first derivative of the state estimate of the displacement in the y direction, is the second-order derivative of the state estimate of the displacement in the x-direction, is the second-order derivative of the state estimate of the displacement in the y direction, represents the product of the first derivative of the state estimate of the displacement in the x-direction and the second derivative of the state estimate of the displacement in the y-direction, Represents the product of the second derivative of the state estimate of the displacement in the x-direction and the first derivative of the state estimate of the displacement in the y-direction.

[0097] Therefore, the relationship between the particle velocity and the motion trajectory curve is:

[0098]

[0099] is the state estimate of the forward velocity of the particle motion, is the state estimate of the yaw speed of the particle motion, is the first derivative of the state estimate of the displacement in the x direction, is the first derivative of the state estimate of the displacement in the y direction, is the second derivative of the state estimate of the displacement in the x direction, is the second derivative of the state estimate of the displacement in the y direction, represents the product of the first derivative of the state estimate of the displacement in the x-direction and the second derivative of the state estimate of the displacement in the y-direction, Represents the product of the second derivative of the state estimate of the displacement in the x-direction and the first derivative of the state estimate of the displacement in the y-direction.

[0100] From formula (1.9), we can see that given a motion trajectory curve, we can determine the forward velocity and yaw speed of the particle motion; conversely, according to formula (1.6), given the forward velocity and yaw speed of the particle motion, we can determine the parametric equation of the particle motion trajectory. Therefore, using formula (1.9), we can convert the parametric curve Expressed in velocity form.

[0101] In fact, for general mechanical systems, the dynamic equations obtained by modeling using the Euler-Lagrange equation are basically about speed. Therefore, after converting the target trajectory curve into a speed target, it is very convenient to design a force or torque controller.

[0102] The specific steps for deriving the dynamic equation of the mobile printing robot using the Euler-Lagrange equation described in step (2) above are as follows:

[0103] (2a) Motion constraints of the three-wheeled mobile structure

[0104] like Figure 2 As shown, the driving motors of the mobile printing structure are installed on the two rear wheels, which drive the left and right wheels to move forward. Let the coordinates of the center O be (x(t), y(t)). When the two-wheeled inverted pendulum does not slip on the plane, O moves along a smooth curve on the plane, so its motion must satisfy the constraint equation (1.2). In addition, when the two wheels do not slip or idle, the following speed relationship is obtained.

[0105]

[0106] in are the rotation speeds of the left and right wheels respectively, v is the forward velocity of the particle motion, is the state estimate of the yaw speed of the particle motion, d is the distance between the left and right wheels, and r is the wheel radius.

[0107] Therefore, the three-moving mobile structure is subject to a total of one nonholonomic constraint (1.2) and two holonomic constraints (2.1) during its movement. From equation (2.1), By inversely solving it and combining it with equation (1.5), the constraint equations of the three-wheeled mobile structure during movement can be summarized as

[0108]

[0109] in are the first-order derivatives of displacement in the x and y directions respectively, θ is the angle between the tangent vector of the printing robot motion curve and the x-axis, are the rotation speeds of the left and right wheels respectively, v is the forward speed of the printing carriage, is the yaw speed of the mobile printing structure, d is the distance between the left and right wheels, and r is the wheel radius.

[0110] In fact, the three-wheeled mobile structure is subject to only three constraints, and equation (2.2) actually contains an assignment relation equation (1.3).

[0111] For the constraints on the front wheels, consider the front end point P(x P ,y P ) movement. Note that point P has two identities. First, as the center point of the front wheel, its motion satisfies the lateral velocity of zero.

[0112]

[0113] in are the first-order derivatives of the displacements in the x and y directions at point p, respectively. It is the angle between the front wheel direction of the mobile printing robot and the middle pole.

[0114] On the other hand, P is the front end point of the intermediate rod, and its movement speed is as follows: Figure 3 As shown, combined with formula (2.3), we have the following relationship

[0115]

[0116] in are the first-order derivatives of the displacements in the x and y directions at point p, respectively. is the yaw speed of the mobile printing structure, is the angle between the front wheel direction of the mobile printing robot and the middle rod, v is the forward speed of the printing robot, and a is the length of the middle rod.

[0117] Rewrite (2.4) as

[0118]

[0119] is the yaw speed of the mobile printing structure, is the angle between the front wheel direction of the mobile printing robot and the middle rod, v is the forward speed of the printing robot, and a is the length of the middle rod.

[0120] Equation (2.5) is a nonholonomic constraint that describes the relationship between the front wheel steering angle and the velocity of the center point O. Substituting into (2.5), the nonholonomic constraint can also be rewritten as

[0121]

[0122] Where θ is the angle between the tangent vector of the printing robot motion curve and the x-axis, is the yaw speed of the mobile printing structure, is the angle between the front wheel direction of the mobile printing robot and the middle rod, a is the length of the middle rod, v is the forward speed of the printing car, are the first derivatives of the displacement in the x direction respectively.

[0123] (2b) Dynamic equations of the three-wheeled mobile structure

[0124] In the three-wheeled mobile structure, It is controlled, Considered as the control variable. Combining (2.2) and (2.5), all the motion constraints on the three-wheeled mobile structure are

[0125]

[0126] in are the first-order derivatives of displacement in the x and y directions respectively, θ is the angle between the tangent vector of the printing robot motion curve and the x-axis, are the rotation speeds of the left and right wheels respectively, v is the forward speed of the printing carriage, is the yaw speed of the mobile printing structure, a is the length of the middle rod, d is the distance between the left and right wheels, r is the wheel radius, It is the angle between the front wheel direction of the mobile printing robot and the middle pole.

[0127] make Write the constraint equation (2.7) in matrix form

[0128]

[0129] in:

[0130] Where q is the state vector of the mobile printing robot, is the first-order derivative of the state vector of the mobile printing robot, S(q) is the constraint matrix of the mobile printing robot, V is the velocity matrix of the mobile printing robot, x and y are the displacements in the x and y directions respectively, θ is the angle between the tangent vector of the printing robot motion curve and the x-axis, θ l ,θ r are the angles between the tangent vectors of the left and right wheel motion curves and the x-axis, v is the forward speed of the printing vehicle, is the yaw speed of the mobile printing structure, d is the distance between the left and right wheels, r is the wheel radius, a is the length of the middle rod, It is the angle between the front wheel direction of the mobile printing robot and the middle pole.

[0131] From the formula (2.7), it seems that the three-wheeled mobile structure is subject to a total of four constraints, but in fact, the constraint equations (2.5) and the second formula (2.2) are not independent of each other, but affect each other. In the case of a two-wheeled mobile structure, the three constraints of the two-wheeled structure are that the lateral speed is zero, the sum of the two wheel speeds determines the forward speed, and the difference between the two wheel speeds determines the yaw speed. In the three-wheeled mobile structure of the present invention, the three constraints are actually that the lateral speed is zero, the sum of the two wheel speeds determines the forward speed, the forward speed v and Determine the yaw speed, the corresponding Therefore, we combine these two constraints and summarize the constraints on the three-wheeled mobile structure into the following form:

[0132]

[0133] Among them, S(q) is the constraint matrix of the mobile printing robot, V is the velocity matrix of the mobile printing robot, are the first-order derivatives of displacement in the x and y directions respectively, θ is the angle between the tangent vector of the printing robot motion curve and the x-axis, is the yaw speed of the mobile printing structure, The speeds of the left and right wheels respectively. is the angle between the front wheel direction of the mobile printing robot and the middle rod, and a is the length of the middle rod.

[0134] Write it in matrix form

[0135]

[0136] in:

[0137]

[0138] F(q) is the constraint matrix of the mobile printing robot after integration, is the first derivative of the state vector of the mobile printing robot, θ is the angle between the tangent vector of the printing robot motion curve and the x-axis, a is the length of the middle rod, and r is the wheel radius.

[0139] Note that F(q) and S(q) satisfy the following relationship

[0140] F 3×5 (q)·S 5×2 (q)=0 3×2 (2.9)

[0141] Among them, F(q) is the constraint matrix of the mobile printing robot after integration, S 5×2 (q) satisfies that the product of the matrix F(q) is a zero matrix.

[0142] In order to use the Euler-Lagrange equation to calculate the dynamic equations of the three-wheeled mobile structure, we first need to calculate the kinetic energy of each part separately. Figure 3 As shown, the following position relationship can be obtained

[0143]

[0144]

[0145] Among them, (x ωl ,y ωl ), (x ωr ,y ωr ) are the center coordinates of the left and right rear wheels, v ωl , v ωr , v C and v P are the speeds of the center points of the rear two wheels, the center point C of the middle rod, and the center point P of the front wheel, respectively. x is the displacement in the x direction, y is the displacement in the y direction, a is the length of the middle rod, and θ is the angle between the tangent vector of the motion curve of the printing robot and the x axis. In this way, the squares of the speeds of each center point can be obtained as follows:

[0146]

[0147]

[0148]

[0149]

[0150] Among them, v ωl , v ωr , v C and v P are the speeds of the center points of the rear two wheels, the center point C of the middle pole and the center point P of the front wheel, respectively. is the first derivative of the displacement in the x direction, is the first derivative of displacement in the y direction, a is the length of the middle rod, θ is the angle between the tangent vector of the printing robot motion curve and the x-axis, is the yaw speed of the mobile printing structure, a is the length of the middle rod, and d is the distance between the left and right wheels.

[0151] Therefore, the sum of the forward kinetic energy and rotational kinetic energy of the two rear wheels is

[0152]

[0153] Among them, M ω is the wheel mass, is the first derivative of the displacement in the x direction, is the first derivative of displacement in the y direction, is the yaw speed of the mobile printing structure, d is the distance between the left and right wheels, The speeds of the left and right wheels, I ω is the moment of inertia of the wheel around the axle, I ωd is the moment of inertia of the wheel about the z-axis.

[0154] The sum of the forward kinetic energy and rotational kinetic energy of the middle pole and the front wheel are

[0155]

[0156]

[0157] Where: M B is the mass of the middle rod, M ω is the wheel mass, I B is the moment of inertia of the middle rod around the z-axis, I ω It represents the moment of inertia of the wheel around the axle. is the first derivative of the displacement in the x direction, is the first derivative of displacement in the y direction, is the yaw speed of the mobile printing structure, r is the wheel radius, and a is the length of the intermediate rod.

[0158] Therefore, the Lagrange function is

[0159] L=T ω +T B +T f (2.10)

[0160] Among them, T ω is the sum of the forward kinetic energy and rotational kinetic energy of the two rear wheels, and the wheel mass, T B is the sum of the forward kinetic energy of the middle rod and the front wheel, T f It is the sum of the rotational kinetic energy of the middle rod and the front wheel.

[0161] Using the Euler-Lagrange equations of nonholonomic mechanical systems

[0162]

[0163] Where λ is the Lagrange multiplier, q is the state vector of the mobile printing robot, is the derivative of the state vector of the mobile printing robot, E(q) is the matching matrix, F(q) is the transposed matrix of the constraint matrix of the mobile printing robot after integration, and T is the input torque vector.

[0164] At this time, the input torque vector T and matching matrix E(q) are respectively

[0165]

[0166] Among them, T l , T r are the input torque vectors of the left and right wheels respectively.

[0167] Substitute (2.10) into equation (2.11) and arrange it according to the derivatives of each order of the state variable to obtain

[0168]

[0169] in:

[0170]

[0171]

[0172]

[0173]

[0174]

[0175] λ is the Lagrange multiplier, q is the state vector of the mobile printing robot, is the derivative of the state vector of the mobile printing robot, is the second-order derivative of the state vector of the mobile printing robot, E(q) is the matching matrix, F(q) is the transposed matrix of the constraint matrix of the mobile printing robot after integration, T is the input torque vector, A(q) is the inertia matrix of the printing robot, Represents the centripetal force and Golgi force matrices of the system, M1, M2, and M B , M ω , I ω , M B The parameter related to r is the mass of the middle rod, M ω is the wheel mass, I is the moment of inertia of the mobile printing robot, and I ω is the moment of inertia of the wheel around the axle, I ωd is the moment of inertia of the wheel around the z-axis, I B is the moment of inertia of the middle rod around the z-axis, a is the length of the middle rod, and r is the radius of the wheel.

[0176] Derivative (2.8) with respect to t, and then substitute into (2.12) to obtain

[0177]

[0178] Where λ is the Lagrange multiplier, V is the velocity matrix, is the derivative of the velocity matrix, S is the constraint matrix of the mobile printing robot for the direction of q value, is the derivative of the constraint matrix of the mobile printing robot for the direction of q value, A is the inertia matrix of the mobile printing robot, represents the centripetal force and Golgi force matrix of the system, T is the input torque vector of the mobile printing robot, and E(q) is the matching matrix

[0179] Multiply both sides of (2.13) by S T (q), and using formula (2.9) to eliminate the Lagrange multiplier, the dynamic equation of the three-wheeled mobile structure is:

[0180]

[0181] in:

[0182]

[0183]

[0184]

[0185] It is controlled by the angle between the front wheel direction and the middle pole of the mobile printing robot. As a control variable, is the front wheel speed of the mobile printing robot, q is the state vector of the mobile printing robot, is the derivative of the state vector of the mobile printing robot, x is the displacement in the x direction, y is the displacement in the y direction, θ is the angle between the tangent vector of the motion curve and the x axis, θ l is the angle between the left wheel and the x-axis, θ r is the angle between the right wheel and the x-axis, v is the forward velocity of the particle, is the yaw speed of the printing robot, d is the distance between the left and right wheels, a is the length of the middle rod, r is the wheel radius, V is the velocity matrix, is the derivative of the velocity matrix, S is the constraint matrix of the mobile printing robot for the direction of q value, is the derivative of the constraint matrix of the mobile printing robot for the direction of q value, S Τ is the transposed matrix of the constraint matrix of the mobile printing robot for the direction of q value, A is the inertia matrix of the mobile printing robot, represents the centripetal force and Golgi force matrix of the system, T is the input torque vector of the mobile printing robot, E(q) is the matching matrix, T l ,T r They are the torques provided by the left and right wheel drives, M1 represents the mass of the intermediate body, and I ωrepresents the moment of inertia of the wheel around the wheel axis, and I represents the moment of inertia of the mobile printing robot.

[0186] (2c) Motion control of three-wheeled mobile structure

[0187] The kinetic equation (2.14) can be written in the form of the equation of state

[0188]

[0189] in:

[0190]

[0191]

[0192] is the angle between the front wheel direction of the mobile printing robot and the middle pole. As a control variable, is the front wheel speed of the mobile printing robot, θ is the angle between the tangent vector of the motion curve and the x-axis, is the second derivative of the angle between the tangent vector of the motion curve and the x-axis, v is the forward velocity of the particle motion, is the first derivative of the forward velocity of the particle motion, d is the distance between the left and right wheels, a is the length of the middle rod, r is the wheel radius, T l ,T r They are the torques provided by the left and right wheels, c is the distance d between the two wheels, the wheel radius r, and the moment of inertia of the wheel around the wheel axis I ω Related parameters, M1 represents the parameters related to the wheel mass radius and moment of inertia, Δ is the change, I ω represents the moment of inertia of the wheel around the wheel axis, I represents the moment of inertia of the mobile printing robot, u1 is the intermediate control variable of the tracking controller, u2 and Determines the two variables of the tracking controller.

[0193] Wheel radius and moment of inertia of the wheel about the wheel axis

[0194] Using equation (1.9) to transform the target trajectory curve into a velocity form, combined with the control system (2.15) to obtain the general trajectory tracking control problem. Note that u1 is the intermediate control variable, and the final control variable to be designed is Since equation (2.15) is decoupled, we can first design u1 based on the second equation, and then combine equation (2.5) to obtain

[0195]

[0196] is the angle between the front wheel direction of the mobile printing robot and the middle rod, v is the forward velocity of the printing robot, a is the length of the middle rod, and u1 is the intermediate control variable of the tracking controller.

[0197] Equation (2.16) gives the control variables u1 and The control variables obtained are Substitute the first equation into (2.15) and redesign the control variable u2 to complete the final trajectory tracking control design task.

[0198] The specific steps of the dynamic tracking target design described in step (3) above are as follows:

[0199] The previous sections have systematically analyzed the constraint equations and dynamic equations for various types of wheeled mobile structures. In fact, constraint equation (1.2) is a nonholonomic constraint that all wheeled mobile structures must satisfy when there is no lateral slip. By analyzing this constraint, we can use equation (1.9) to transform the task trajectory curve of the wheeled mobile structure into a velocity target. Then, combined with its dynamic equations, we can transform the original motion task into a general trajectory tracking control problem.

[0200] However, no matter how advanced the control design method, the actual motion trajectory will always deviate from the target trajectory curve, especially during the initial period. One of the main reasons for this is that the speed target (1.9) used has a relatively large difference compared to the actual speed at the initial moment. This speed error accumulates into a large position error. In addition, if a static speed target (1.9) is used, when the forward speed error control system is affected by unknown disturbances, the actual forward speed will deviate from the given forward speed target. At this time, the yaw speed target cannot be constantly adjusted, which will cause the wheeled mobile structure to deviate from the target trajectory curve. To solve these two problems simultaneously and enable the wheeled mobile structure to move accurately along the given target trajectory curve, we improve the static speed target (1.9) to a dynamic tracking target.

[0201] (3a) Dynamic tracking target design

[0202] For a given target trajectory curve Use (1.9) to convert it into a speed target. Further rewrite it into

[0203]

[0204] in is the relative curvature of the target curve.

[0205] is the tangent vector length of the curve, and its physical meaning is the forward speed of the printing robot. is the state estimation value of the angle between the tangent vector of the printing robot motion curve and the x-axis, is the state estimate of the yaw speed of the printing robot, is the first-order derivative of the state estimate of the x-direction displacement of the printing robot, is the first-order derivative of the state estimate of the printing robot’s y-direction displacement, is the second-order derivative of the state estimate of the x-direction displacement of the printing robot, is the second-order derivative of the state estimate of the printing robot’s y-direction displacement, Represents the product of the first-order derivative of the state estimation value of the printing robot's x-direction displacement and the second-order derivative of the state estimation value of the y-direction displacement, This represents the product of the second derivative of the estimated x-direction displacement of the printing robot and the first derivative of the estimated y-direction displacement. s(t) is the trajectory of the printing robot integrated from time 0 to time t, and k(s(t)) is the relative curvature of the printing robot's trajectory.

[0206] In fact, the curvature function k(s(t)) is the curve When the parameter t = s is the arc length parameter, the forward velocity target When the speed is unit, the curvature function k(s) uniquely determines a smooth curve on the plane. When t is not an arc length parameter, the forward speed And the curvature function k(t)=k(s(t)) can uniquely determine a smooth curve on the plane. Introducing a new time variable η, which is determined by t = φ(η), where φ is a one-to-one smooth function, the original parameter curve can be transformed into Derivative relations from composite functions

[0207]

[0208] Among them, η is a new time variable t=φ(η) introduced by a one-to-one corresponding smooth function φ(η).

[0209] Combining equation (3.1) we get The forward speed is

[0210]

[0211] in, is the forward velocity of the printing robot at time, is a smooth function of the time variable η, is the obtained forward velocity with respect to the time variable η.

[0212] As can be seen from the above equation, the forward velocity target in equation (3.1) can be adjusted through parameter transformation. Therefore, the second equation in equation (3.1) involving the curve curvature is the essential description of the trajectory curve.

[0213] In the actual tracking process, if we only need the wheeled mobile structure to accurately follow the given trajectory curve If the speed of the movement is not important, then we can give up the precise tracking of the forward speed target and focus on tracking the second equation of equation (3.1). At this time, the tracking target (3.1) can be improved to a dynamic tracking target

[0214]

[0215] in, is the first derivative of a one-to-one corresponding smooth function, s is the trajectory curve of the printing robot, k(s) is the relative curvature of the trajectory curve of the printing robot, is the yaw speed of the mobile printing structure.

[0216] Here, the design can be made according to actual needs, v(t) is the actual forward velocity, and Equation (3.2) can be called a dynamic tracking target.

[0217] Generally speaking, φ(t) can be designed as

[0218] φ(t)=-(βt+1)le- βt +l

[0219] Among them, l is the length of the target trajectory curve, β is an adjustable parameter, is the first derivative of the corresponding smooth function, and e is an irrational number.

[0220] at this time

[0221]

[0222] Among them, l is the length of the target trajectory curve, β is an adjustable parameter, is the first derivative of the corresponding smooth function, and e is an irrational number.

[0223] In practical applications, since the wheeled mobile structures are all stationary at the initial moment, the forward velocity target (3.3) can be used to make the initial velocity error of the tracking control problem zero, and the parameters can be adjusted according to actual needs to achieve the best tracking control effect throughout the process, such as Figure 4 ,5,6 as shown.

Claims

1. A method for dynamically tracking the motion trajectory of a mobile printing robot, characterized in that: The following steps are involved: Step 1: Analyze the motion characteristics of the particle along the smooth curve, obtain the relationship between the particle's motion speed and the actual motion trajectory curve, and obtain the motion law of the mobile printing robot; Step 2: Based on the constraint relationship between the wheel speed and the movement speed of the mobile printing robot, the holonomic and nonholonomic constraints of the mobile printing robot structure are obtained. The dynamic equation of the mobile printing robot is derived by combining the holonomic and nonholonomic constraints of the mobile printing robot structure using the Euler-Lagrange equation. The target trajectory curve of the mobile printing robot is converted into the speed target of the mobile printing robot according to the dynamic equation. The dynamic equation of the mobile printing robot is: (2.13) in: ； ； is the angle between the front wheel direction of the mobile printing robot and the middle pole. As a control variable, is the front wheel speed of the mobile printing robot, is the state vector of the mobile printing robot, is the derivative of the state vector of the mobile printing robot, yes Directional displacement, yes Directional displacement, is the tangent vector of the motion curve and The angle between the axes, It is a revolver and The angle between the axes, It is the right wheel and The angle between the axes, is the forward velocity of the particle, is the yaw speed of the printing robot, is the distance between the left and right wheels, a is the length of the middle pole, is the wheel radius, is the velocity matrix, is the derivative of the velocity matrix, Is a mobile printing robot The constraint matrix of the value direction, Is a mobile printing robot The derivative of the constraint matrix in the direction of the value, Is a mobile printing robot The transposed matrix of the constraint matrix of the direction of the value, is the inertia matrix of the mobile printing robot, represents the centripetal force and Göring force matrix of the system, is the input torque vector of the mobile printing robot, is the matching matrix, are the torques provided by the left and right wheel drives, Represents parameters related to wheel mass radius and moment of inertia, represents the moment of inertia of the wheel around the axle, represents the moment of inertia of the mobile printing robot; According to the dynamic equation, the target trajectory curve of the mobile printing robot is converted into the speed target of the mobile printing robot, specifically including: The dynamic equation is converted into the state equation of the mobile printing robot: (2.14) in: ； is the angle between the front wheel direction of the mobile printing robot and the middle pole. As a control variable, is the front wheel speed of the mobile printing robot, is the tangent vector of the motion curve and The angle between the axes, is the tangent vector of the motion curve and The second derivative of the angle between the axes, is the forward velocity of the particle, is the first derivative of the forward velocity of the particle motion, is the distance between the left and right wheels, a is the length of the middle pole, is the wheel radius, are the torques provided by the left and right wheel drives, Is the distance between the two wheels , wheel radius , and the moment of inertia of the wheel about the axle Related parameters, Represents parameters related to wheel mass radius and moment of inertia, is the amount of change, represents the moment of inertia of the wheel around the axle, represents the moment of inertia of the mobile printing robot, is the intermediate control variable of the tracking controller, and Determines the two variables of the tracking controller; Using the relationship between the velocity and trajectory of the particle in step 1, the target trajectory curve is converted into a velocity form; Step 3: Calculate the relative curvature based on the speed target of the mobile printing robot, introduce the relative curvature to design the dynamic trajectory tracking target, and combine the motion law and dynamic equation of the mobile printing robot through the dynamic trajectory tracking target to design a controller.

2. The method for dynamically tracking the motion trajectory of a mobile printing robot according to claim 1, characterized in that: In step 1, the characteristics of the particle motion along the smooth curve are analyzed, specifically: （1.4） in, To print the tangent vector of the robot motion curve and The angle between the axes, They are and The first derivative of the displacement in the direction.

3. The method for dynamically tracking the motion trajectory of a mobile printing robot according to claim 1, characterized in that: In step 1, the relationship between the velocity of the particle and the actual motion trajectory curve is as follows: Given a smooth curve , the curve is regarded as the motion trajectory of a certain particle; by taking the derivative of t, the relationship between the velocity of the particle and the motion trajectory curve is as follows: （1.9） in, is the tangent vector length of the curve, and its physical meaning is the forward velocity of the particle motion. is the tangent vector of the motion curve and The state estimate of the axis angle, is the state estimate of the yaw speed of the particle motion, yes The state estimate of the directional displacement, yes The state estimate of the directional displacement, yes The first derivative of the state estimate of the directional displacement, yes The first derivative of the state estimate of the directional displacement, yes The second derivative of the state estimate of the directional displacement, yes The second derivative of the state estimate of the directional displacement, express The first derivative and the state estimate of the directional displacement The product of the second derivatives of the state estimates of the directional displacement, express The second derivative and the state estimate of the directional displacement The product of the first derivatives of the state estimates of the directional displacements.

4. The method for dynamically tracking the motion trajectory of a mobile printing robot according to claim 1, characterized in that: In step 3, the relative curvature is calculated based on the speed target of the mobile printing robot, specifically: For a given target trajectory curve, the relationship between the velocity of the particle and the trajectory is used to convert it into a velocity target: ； in is the relative curvature of the target curve; is the tangent vector length of the curve, and its physical meaning is the forward speed of the printing robot. is the tangent vector of the printed robot motion curve and The state estimate of the axis angle, is the state estimate of the yaw speed of the printing robot, It's a printing robot The first derivative of the state estimate of the directional displacement, It's a printing robot The first derivative of the state estimate of the directional displacement, It's a printing robot The second derivative of the state estimate of the directional displacement, It's a printing robot The second derivative of the state estimate of the directional displacement, Printing robot The first derivative and the state estimate of the directional displacement The product of the second derivatives of the state estimates of the directional displacement, Printing robot The second derivative and the state estimate of the directional displacement The product of the first derivative of the state estimate of the directional displacement, Is the printing robot from 0 to time The trajectory curve obtained by integration is: is the relative curvature of the printing robot trajectory curve.

5. The method for dynamically tracking the motion trajectory of a mobile printing robot according to claim 4, characterized in that: In step 3, relative curvature is introduced to design a dynamic trajectory tracking target. The motion law of the mobile printing robot and the dynamic equation are combined through the dynamic trajectory tracking target to design a controller, which specifically includes: ； is the forward speed of the printing robot, is the tangent vector of the printed robot motion curve and The state estimate of the axis angle, is the state estimate of the yaw speed of the printing robot, is the corresponding smooth function, Is the printing robot from 0 to time The trajectory curve obtained by integration is: is the relative curvature of the printing robot trajectory curve; Design according to actual needs, is the actual forward velocity; Designed for ； is the length of the target trajectory curve, is an adjustable parameter. is the corresponding smooth function, is an irrational number; ； is the length of the target trajectory curve, is an adjustable parameter. is the first derivative of the corresponding smooth function, is an irrational number; Dynamic target tracking is implemented as follows: Convert the target trajectory curve into speed form and combine it with the state equation of the mobile printing robot to realize trajectory tracking control. It is an intermediate control variable. The state equation is decoupled. First, design it according to the state equation. , and then combined with the nonholonomic constraint equation: (2.15) in, is the yaw speed of the printing robot, is the forward velocity of the particle, is the angle between the front wheel direction of the mobile printing robot and the middle pole, a is the length of the middle pole, is the intermediate variable of the controller, Is the distance between the two wheels , wheel radius , and the moment of inertia of the wheel around the wheel axis Related parameters; Equation (2.15) gives the control variable and The relationship between the control variables Substitute into equation (2.14) and design the control variables , complete the controller design and realize the final trajectory tracking control task.

Images