A differential chassis-oriented AGV trolley curve trajectory deviation calculation method

Abstract

The application discloses a kind of AGV trolley trajectory deviation calculation method for differential chassis, it belongs to the field of electromechanical control technology.The present application solves the problem of existing method, such as trajectory is not adjustable, poor motion stability.The method of the present application first constructs the kinematic model and control model of AGV trolley of differential chassis, and establishes trajectory deviation calculation model based on kinematic model, control model and cascade PID control.According to trajectory deviation calculation model, the deviation between the predetermined trajectory and the actual trajectory during the process of AGV trolley of differential chassis passing through the bend can be calculated, and the trajectory deviation value of each trajectory is obtained, and then the best trajectory is selected, which helps to improve the motion control accuracy and stability of AGV trolley.The method of the present application can be applied to the trajectory selection of AGV trolley of differential chassis.

Description

Technical Field

[0001] This invention belongs to the field of electromechanical control technology, specifically relating to a method for calculating the cornering trajectory deviation of an AGV (Automated Guided Vehicle) for a differential chassis. Background Technology

[0002] Due to the significant increase in the number of newly built automated warehouses in recent years, the demand for AGVs, as an important warehouse automation equipment in my country, is also increasing. As a result, the design and manufacturing of AGVs has developed rapidly. However, with the widespread use of AGVs, some problems have gradually emerged. Especially when the warehouse environment is relatively complex, in order for the AGV to reach the target location according to the predetermined route, it is necessary to keep the deviation within a controllable range. In actual operation, the biggest deviation occurs during the turning process. If the deviation during the turning process is too large, it will cause a large deviation in the subsequent route, thereby affecting the working efficiency of the AGV.

[0003] In the field of AGV (Automated Guided Vehicle) rapid cornering technology, researchers mainly employ fuzzy control, PID control, and model predictive control to achieve rapid cornering. Regarding the cornering trajectory, existing AGVs primarily use a perfect circular arc trajectory, which suffers from problems such as a large turning radius, non-adjustable trajectory, and poor motion stability during cornering. Therefore, proposing a new control method to address these issues is currently a pressing need. Summary of the Invention

[0004] This invention addresses the problems of unadjustable trajectory and poor motion stability in existing cornering methods by proposing a method for calculating the cornering trajectory deviation of AGVs with differential chassis.

[0005] The technical solution adopted by this invention to solve the above-mentioned technical problems is: a method for calculating the cornering trajectory deviation of an AGV (Automated Guided Vehicle) for a differential chassis, the method specifically including the following steps:

[0006] Step 1: Establish the kinematic model of the AGV with a differential chassis, and then establish the control model of the AGV based on the kinematic model of the AGV.

[0007] Step 2: Control the AGV based on its control model and PID controller, and calculate the trajectory deviation based on the AGV's motion trajectory and the preset trajectory.

[0008] Step 3: Select the cornering trajectory based on the maximum deviation during cornering and the deviation at the end of cornering for each preset trajectory.

[0009] Furthermore, the process of establishing the kinematic model is as follows:

[0010] Coordinates of the center of the car satisfy:

[0011] (2)

[0012] in, This indicates the position of the car's center in the global coordinate system. Axial coordinates, This indicates the position of the car's center in the global coordinate system. Axial coordinates, express The first derivative, express The first derivative, This indicates the direction of the car's movement relative to the global coordinate system. Angle along the axial direction;

[0013] According to equation (2), we get:

[0014] (3)

[0015] in, This represents the linear velocity at the center of the car. This represents the angular velocity at the center of the car. express The first derivative;

[0016] The linear velocities of the left and right drive wheels are respectively:

[0017] (4)

[0018] in, This represents the radius of curvature of the instantaneous trajectory of the car. This indicates the linear velocity of the left drive wheel. This represents the linear velocity of the right drive wheel. Indicates intermediate variables;

[0019] Convert the speeds of the left and right drive wheels into the angular velocity of the vehicle's center:

[0020] (5)

[0021] Convert the speeds of the left and right drive wheels into the linear velocity of the vehicle's center:

[0022] (6)

[0023] For the inverse kinematics problem of the trolley, according to equations (5) and (6), the velocities of the left and right driving wheels of the trolley can be expressed by the linear velocity and angular velocity of the center of the trolley as follows:

[0024] (7)

[0025] Combining formulas (3) and (7), the kinematic model of the AGV is obtained as follows:

[0026] (8)

[0027] in, This indicates the initial azimuth angle of the vehicle. This represents the initial position vector of the vehicle. Indicates time;

[0028] The discretized state equation of the AGV is:

[0029] (10)

[0030] in, This indicates that the left drive wheel of the AGV is in The speed of time This indicates that the right drive wheel of the AGV is in The speed of time This represents the time interval between two adjacent moments. Indicates that the AGV cart is in Azimuth at time, This indicates that the center of the AGV is at... Location at any given moment This indicates that the center of the AGV is at... Location at any given moment Indicates that the AGV cart is in The azimuth at any given time.

[0031] Furthermore, the intermediate variable for:

[0032] (1)

[0033] in, Indicates the left drive wheel in the global coordinate system Position vector in the plane Indicates the right drive wheel in the global coordinate system Position vector in the plane.

[0034] Furthermore, the control model of the AGV includes an inverse kinematics control model and a forward kinematics control model.

[0035] Furthermore, the process of establishing the inverse kinematics control model is as follows:

[0036] Calculate the linear velocity and angular velocity of the AGV center based on the known trajectory and formula (3), and then substitute the linear velocity and angular velocity of the AGV center into formula (7) to obtain the velocity of the left drive wheel and the velocity of the right drive wheel.

[0037] Based on the speed of the left drive wheel, the functional relationship between the rotational speed of the left drive wheel and time can be obtained; similarly, based on the speed of the right drive wheel, the functional relationship between the rotational speed of the right drive wheel and time can be obtained.

[0038] (11)

[0039] in, express The rotational speed of the left drive wheel at any given moment. express The rotational speed of the right drive wheel at any given moment. Indicates the radius of the wheel;

[0040] The functional relationship between the input armature terminal voltage and the rotational speed is obtained through analysis of the DC motor:

[0041] (12)

[0042] in, This represents the input armature terminal voltage. Indicates rotational speed;

[0043] Substituting the functional relationship between rotational speed and time into equation (12), we obtain the functional relationship between the input armature terminal voltage and time, thus obtaining the inverse kinematics control model of the trolley.

[0044] Furthermore, the process of establishing the positive kinematic control model is as follows:

[0045] The armature voltage of the motor is denoted as The electromechanical time constant is denoted as Then the relationship between the rotational speed of the drive wheel driven by the DC motor and the armature terminal voltage is:

[0046] (13)

[0047] in, Indicates the rotational speed of the drive wheel. Indicates the armature terminal voltage. Indicates gain. Represents the Laplace variable;

[0048] Substituting the rotational speed calculated by equation (13) into equation (11) yields the speeds of the left and right driving wheels. Substituting the speeds of the left and right driving wheels into equation (10) yields the trajectory of the vehicle, thus obtaining the forward kinematic control model of the vehicle.

[0049] Furthermore, the gain for:

[0050] (14)

[0051] in, This represents the torque constant of the motor. This represents the electromotive force constant of the motor.

[0052] Furthermore, the control of the AGV based on the AGV's control model and the PID controller specifically involves:

[0053] Step 2: 1. Use the output of the second PID controller as the input of the amplifier, then substitute the amplifier's output into the forward kinematics control model to obtain the speed of the driving wheel. Divide the speed of the driving wheel by... The result of the division is recorded as Then divide the result Multiply The result of the multiplication is denoted as Then multiply the result and Multiply them to get the final result. ;

[0054] Step 22: Calculate the correction signal based on the actual trajectory of the AGV and the inverse kinematics control model. ;

[0055] Steps two and three: The calibration signal The final multiplication result The difference is calculated, and the result is used as the input to the first PID controller.

[0056] Step 24: Subtract the output of the first PID controller The difference result is used as the input of the second PID controller, and the process returns to step two-one to achieve closed-loop control of the AGV.

[0057] Furthermore, the preset trajectory includes circular arc curves, third-order Bézier curves, fifth-order Bézier curves, third-order B-spline curves, and fifth-order B-spline curves.

[0058] Furthermore, the step of selecting a cornering trajectory based on the maximum deviation during cornering and the deviation at the end of cornering for each preset trajectory specifically involves:

[0059] To set the maximum deviation threshold when cornering, first select curves whose maximum deviation when cornering is less than the set threshold, then compare the deviations at the end of the cornering of each selected preset curve, and select the preset trajectory with the smallest deviation at the end of the cornering.

[0060] The beneficial effects of this invention are:

[0061] This invention first constructs a kinematic and control model of an AGV (Automated Guided Vehicle) with a differential chassis, and then establishes a trajectory deviation calculation model based on the kinematic, control, and cascade PID control models. According to the trajectory deviation calculation model, the deviation between the predetermined trajectory and the actual trajectory of the AGV during cornering can be calculated, yielding the trajectory deviation value for each trajectory. This invention also conducts simulation experiments using various common curves as examples, providing assistance in finding a certain degree of optimal cornering trajectory. This helps improve the motion control accuracy and stability of the AGV, solves the problem of non-adjustable trajectory in existing circular arc trajectory cornering methods, and effectively reduces the path deviation of the AGV during cornering by selecting the cornering curve, thus improving work efficiency and contributing to the efficiency and accuracy of modern logistics distribution. Attached Figure Description

[0062] Figure 1 This is a flowchart of a method for calculating the cornering trajectory deviation of an AGV (Automated Guided Vehicle) oriented towards a differential chassis, according to the present invention.

[0063] Figure 2 This invention constructs a differential model chassis body vehicle motion model;

[0064] Figure 3 This is a schematic diagram of the movement state and deviation of the vehicle constructed according to the present invention;

[0065] Figure 4 This is a block diagram of the dynamic characteristics structure of the AGV vehicle constructed in this invention;

[0066] Figure 5 It is an equivalent diagram for making small deviations explicit;

[0067] Figure 6 This is a control object variable block diagram constructed by the present invention;

[0068] Figure 7 This is a simulation process diagram of the cascaded PID controller system built according to the present invention. Detailed Implementation

[0069] For a differential model chassis containing a pair of driving wheels and a pair of following wheels, the method of this invention simplifies some constraint relationships during the model building process, specifically based on the following two assumptions:

[0070] First, it is assumed that all components of the car are rigid bodies and will not deform.

[0071] Second, the contact between the car wheels and the ground during operation is point contact, and there is no relative sliding between the car wheels and the ground, which is pure rolling.

[0072] like Figure 2 As shown, in order to represent the motion and spatial state of the vehicle at any given time, this invention establishes a ground-based global coordinate system. and the local coordinate system based on the car body ,in, The positive direction of the axis is the direction in which the trolley moves forward. shaft and The axis is perpendicular. The positive direction of the axis is due east. The positive direction of the axis is due north, and plane and Planes are parallel.

[0073] The forward kinematic process can be described as follows: the speed of the left drive wheel of the trolley... and the speed of the right drive wheel Convert to global coordinate system Linear velocity of the center of the car in the plane and angular velocity And based on linear velocity and angular velocity Obtain the trajectory of the car;

[0074] The inverse kinematics process is described as follows: Given a global coordinate system The trajectory of the cart in the plane yields the linear velocity of the cart's center. and angular velocity Then, the linear velocity of the center of the car. and angular velocity Converted to the speed of the left drive wheel of the car and the speed of the right drive wheel .

[0075] The method of the present invention will be described in detail below with reference to the accompanying drawings:

[0076] Specific implementation method one: Combining Figure 1 This embodiment describes a method for calculating the cornering trajectory deviation of an AGV (Automated Guided Vehicle) with a differential chassis. The method specifically includes the following steps:

[0077] Step 1: Establish the kinematic model of the AGV with a differential chassis, and then establish the control model based on the kinematic model of the AGV.

[0078] Specifically, the process of establishing the kinematic model is as follows:

[0079] Position the left drive wheel in the global coordinate system The position vector in the plane is denoted as Place the right drive wheel in the global coordinate system The position vector in the plane is denoted as From the first assumption, we can conclude that:

[0080] (1)

[0081] in, Indicates intermediate variables;

[0082] Furthermore, according to the second assumption, the direction of motion of the trolley is always perpendicular to the axis of the driving wheel; therefore, the coordinates of the trolley's center are... satisfy:

[0083] (2)

[0084] in, This indicates the position of the car's center in the global coordinate system. Axial coordinates, This indicates the position of the car's center in the global coordinate system. Axial coordinates, express The first derivative, express The first derivative, This indicates the direction of the car's movement relative to the global coordinate system. Angle along the axial direction;

[0085] According to equation (2), we get:

[0086] (3)

[0087] in, This represents the linear velocity at the center of the car. This represents the angular velocity at the center of the car. express The first derivative;

[0088] With point The instantaneous center of velocity of the car is given by Let be the radius of curvature of the instantaneous trajectory of the cart. It can be seen that the relationship between the velocities of various points on the cart is based on... Starting from the same point and linearly distributed with the same slope, according to the linear distribution law of velocity, the magnitudes of the linear velocities of the left and right driving wheels are respectively:

[0089] (4)

[0090] in, This represents the radius of curvature of the instantaneous trajectory of the car. This indicates the linear velocity of the left drive wheel. This indicates the linear velocity of the right drive wheel;

[0091] Next, based on the forward kinematics problem of the car, the velocities of the left and right driving wheels are converted into the angular velocity of the car's center:

[0092] (5)

[0093] Based on the properties of rigid bodies, we know that the rotation center of all points on the trolley is the same. We can convert the velocities of the left and right driving wheels into the linear velocity of the trolley's center:

[0094] (6)

[0095] For the inverse kinematics problem of the trolley, according to equations (5) and (6), the velocities of the left and right driving wheels of the trolley can be expressed by the linear velocity and angular velocity of the center of the trolley as follows:

[0096] (7)

[0097] In practical operation, the encoders of the differential chassis AGV are usually installed on the left and right drive wheels. Through the encoder feedback from the left and right drive wheels, their velocities can be obtained. Thus, through forward kinematics, the initial position vector of the AGV is known. and initial azimuth angle Then, combining formulas (3) and (7), the kinematic model of the AGV is obtained as follows:

[0098] (8)

[0099] in, This indicates the initial azimuth angle of the vehicle. This represents the initial position vector of the vehicle. , and All are quantities that change over time;

[0100] Equation (8) yields the calculated positioning formula for the AGV based on the kinematic model:

[0101] (9)

[0102] Discretizing equation (9) yields the state equation of the AGV:

[0103] (10)

[0104] in, This indicates that the left drive wheel of the AGV is in The speed of time This indicates that the right drive wheel of the AGV is in The speed of time This represents the time interval between two adjacent moments. Indicates that the AGV cart is in Azimuth at time, This indicates that the center of the AGV is at... Location at any given moment This indicates that the center of the AGV is at... Location at any given moment Indicates that the AGV cart is in The azimuth at any given time.

[0105] The control model of the AGV includes an inverse kinematics control model and a forward kinematics control model. The establishment processes of the inverse kinematics control model and the forward kinematics control model are as follows:

[0106] 1. Inverse kinematics control model

[0107] Calculate the linear velocity and angular velocity of the AGV center based on the known trajectory and formula (3), and then substitute the linear velocity and angular velocity of the AGV center into formula (7) to obtain the velocity of the left drive wheel and the velocity of the right drive wheel.

[0108] Based on the speed of the left drive wheel, the functional relationship between the rotational speed of the left drive wheel and time can be obtained; similarly, based on the speed of the right drive wheel, the functional relationship between the rotational speed of the right drive wheel and time can be obtained.

[0109] (11)

[0110] in, express The rotational speed of the left drive wheel at any given moment. express The rotational speed of the right drive wheel at any given moment. Indicates the radius of the wheel;

[0111] The functional relationship between the input armature terminal voltage and the rotational speed is obtained through analysis of the DC motor:

[0112] (12)

[0113] in, This represents the input armature terminal voltage. Indicates rotational speed;

[0114] Substituting the functional relationship between rotational speed and time into equation (12), we obtain the functional relationship between the input armature terminal voltage and time, which gives us the inverse kinematics control model of the trolley. From this, we obtain the armature terminal voltage function required to achieve the predetermined trajectory.

[0115] 2. Forward Kinematic Control Model

[0116] The dynamic response characteristics of a motor system are typically determined by both the circuitry and mechanical components. However, for the speed control of a DC motor, due to the low control frequency, factors such as inertia and friction have a relatively small impact on its dynamic response under low-frequency control, allowing it to be approximated as a first-order system. The armature voltage of the motor can be denoted as... The electromechanical time constant is denoted as Then the relationship between the rotational speed of the drive wheel driven by the DC motor and the armature terminal voltage is:

[0117] (13)

[0118] in, Indicates the rotational speed of the drive wheel. Indicates the armature terminal voltage. Indicates gain. Represents the Laplace variable;

[0119] Gain for:

[0120] (14)

[0121] in, This represents the torque constant of the motor. This represents the electromotive force constant of the motor;

[0122] Substituting the rotational speed calculated by equation (13) into equation (11) yields the speeds of the left and right driving wheels. Substituting the speeds of the left and right driving wheels into equation (10) yields the trajectory of the vehicle, thus obtaining the forward kinematic control model of the vehicle.

[0123] In actual operation, the AGV moves along a predetermined path. If its initial motion state is without deviation, that is, the AGV is on the predetermined path, after a certain time... Subsequently, due to external disturbances, the AGV deviated from the preset path. The deviation was mainly reflected in the perpendicular distance between the midpoint of the line connecting the two drive wheels of the AGV and the center line of the preset path. And the angle between the perpendicular bisector of the line connecting the two drive wheels of the trolley and the center line of the preset path. ,like Figure 3 As shown.

[0124] Further analysis based on the kinematic model of the AGV reveals that when the resistance experienced by the two wheels during movement is different, a speed difference is generated between the left and right drive wheels, thus causing a turning angle. ,when When the path is very small, the predetermined path can be considered as a straight line, then:

[0125] (15)

[0126] (16)

[0127] Differentiating equations (15) and (16) with respect to time, we have:

[0128] (17)

[0129] (18)

[0130] Since the path of the car is continuous during the actual movement, integrating the above equation yields the motion relationship of the car throughout the entire process:

[0131] (19)

[0132] (20)

[0133] when When I was very young, At this point, the above formula can be transformed into:

[0134] (twenty one)

[0135] The AGV uses two DC motors to drive two drive wheels respectively. During movement, the wheel speeds of the left and right drive wheels are functions of time. From the derivation in the mathematical modeling section above, the functional relationship between the AGV's motion deviation and time can be obtained. Combined with the AGV's forward kinematics control model, the dynamic characteristic structure diagram of the AGV can be established as follows: Figure 4 As shown.

[0136] Due to the existence of dynamic characteristic block diagrams of AGV and The multiplication element makes it a nonlinear system, making precise design of the control system difficult. However, since the vehicle runs on a given continuous trajectory, its input signal is also given. Therefore, when controlling the vehicle, slight deviations must first be detected, then the deviation information is fed back to the controller, which then sends corresponding correction signals. This allows the nonlinear system of the AGV to be transformed into a linear system through small deviation linearization.

[0137] In actual operation, if the trolley operates under normal conditions, the automatic control system will be in a stable working state, i.e., a balanced state. At this time, the controlled variable and the expected value remain consistent within a certain range. When there is no deviation or the deviation is small, the automatic control system does not make adjustments. Once the detection element detects a signal, indicating a certain degree of deviation between the controlled variable and the expected value, the automatic control system starts working to reduce and eliminate this deviation. To simplify the derivation, it is assumed that the trolley's motion is a uniform linear motion process, and the armature voltages applied to the left and right drive wheel motors are equal. At this moment, the velocity of the center point C of the car It can be considered as a constant, at which point the deviation signal... and All are zero. When an external disturbance causes the trolley to deviate from the predetermined path, the given voltage is adjusted by adding or subtracting a correction control amount from the armature voltage based on the feedback signal, i.e.:

[0138] When the car is about to turn left:

[0139] (twenty two)

[0140] When the car needs to turn right:

[0141] (twenty three)

[0142] It should be noted that, The calculation is based on the inverse kinematics control model, and the specific calculation process is as follows:

[0143] Step 1: Calculate the velocities of the left and right drive wheels based on the current trajectory, and the velocities of the left and right drive wheels also satisfy the following:

[0144] (twenty four)

[0145] (25)

[0146] Step 2: Obtain the speeds of the left and right drive wheels. and ,Will and To do the work, one gets .

[0147] so, Figure 4 The two sums on the right side of the middle column, from top to bottom, are respectively... and .Depend on and The generated deviation signal and ,and Figure 5 Controlled by small deviations and The generated output signal and Similarly, by making small deviations explicit, the equivalent dynamic characteristic structure diagram of the AGV is obtained, due to the average speed It is a constant, so it can be moved into Within the boxes representing the steps, the flowchart can be simplified to... Figure 6 .

[0148] Step 2: Control the AGV based on its control model and PID controller, and calculate the trajectory deviation based on the AGV's motion trajectory and the preset trajectory.

[0149] Specifically, such as Figure 7 As shown:

[0150] The control of the AGV vehicle based on the control model and PID controller is specifically as follows:

[0151] Step 2: 1. Use the output of the second PID controller as the input of the amplifier, then substitute the amplifier's output into the forward kinematics control model to obtain the speed of the driving wheel. Divide the speed of the driving wheel by... The result of the division is recorded as Then divide the result Multiply The result of the multiplication is denoted as Then multiply the result and Multiply them to get the final result. ;

[0152] Step 22: Calculate the correction signal based on the actual trajectory of the AGV and the inverse kinematics control model. ;

[0153] Steps two and three: The calibration signal The final multiplication result The difference is calculated, and the result is used as the input to the first PID controller.

[0154] Step 24: Subtract the output of the first PID controller The difference result is used as the input of the second PID controller. Then, return to step 21 to realize the closed-loop control of the AGV. Follow the method in this step to control the AGV when it turns, and obtain the maximum deviation when turning and the deviation when turning.

[0155] Step 3: Select the cornering trajectory based on the maximum deviation during cornering and the deviation at the end of cornering for each preset trajectory.

[0156] Specifically, in order to solve the problem of the trajectory of a circular arc, the method of the present invention constructs two types of curves: Bézier curves and B-spline curves.

[0157] A Bézier curve consists of a start point, an end point, and control points. The shape of the Bézier curve can be changed by adjusting the control points. There is a strict relationship between the order of a Bézier curve and the number of control points: the order equals the number of control points minus one. This means that for an AGV (Automated Guided Vehicle) with an S-shaped curve to achieve a 90-degree turn at a bend, at least three control points are needed. Therefore, the order of the Bézier curve is at least second order. To change the smoothness of the curve, additional control points can be added without affecting the angle between the input and output ends, thus increasing the order of the curve.

[0158] The formula for an nth-order Bézier curve is as follows:

[0159]

[0160] In the formula, The proportion of each point, The number of control points, Let these be the coordinates of a point on the curve. For the first The coordinates of the control points;

[0161] By comparing the formulas for Bézier curves of different orders, the constants a, b, and c in the expression of a Bézier curve can be represented as follows: ;

[0162] In this equation, b decreases from (n-1) to 0, and c increases from 0 to (n-1), which are powers of (1-t) and t, respectively. The change in a follows the same pattern as Pascal's Triangle. Simulation software is used to determine the step size... and number of control points Determine the values ​​of a, b, and c respectively, and represent the x-coordinate and y-coordinate of each point on the curve;

[0163] Finally, by inputting the coordinates of the control points and the specific step size, drawing and connecting each point, you can obtain Bézier curves of different orders.

[0164] In the actual path design process, there may be situations where the AGV car has excessive angular acceleration near a certain point, or when avoiding obstacles, a certain point on the path may coincide with an obstacle. In such cases, it is necessary to change the position and number of control points. However, the characteristics of the Bézier curve make it impossible for designers to control the direction of the curve locally. Moving or adding or removing control points will cause the entire curve to change.

[0165] B-spline curves can have their shape locally controlled by control points. Their formula is similar to that of Bézier curves. A B-spline curve is shown below:

[0166]

[0167] In the formula, The proportion of each point, The number of control points, Let these be the coordinates of a point on the curve. Let be the degrees of freedom of the B-spline curve, i.e., the degree of the curve, and It refers to the order of the curve. The polynomial coefficients are the weights of the control point coordinates. The coordinates of each control point;

[0168] Unlike Bézier curves, B-spline curves have a more flexible polynomial power, which is not consistent with the number of control points, and The value of is no longer fixed at [0,1], but falls between the maximum and minimum node values. This introduces the concept of nodes compared to Bézier curves; these nodes are a set of numbers equal to the sum of the curve's degree and the number of control points plus one. The parameters are then linked through these nodes. The domain is divided into six intervals.

[0169] The key to designing B-spline curves lies in the polynomial... The calculation can be performed according to the Cox-deBoor recursive formula, that is:

[0170]

[0171]

[0172] In the formula, For node vectors, the property of non-decreasing is that... If it is a uniform periodic B-spline curve, the values ​​are generally a series of values ​​starting from 0 and with an interval of 1, such as [0,1,2,3,4,5,6].

[0173] Using the Cox-deBoor recursive formula, a trigonometric calculation format similar to Pascal's triangle can be derived, and on this basis, the construction of an nth-degree B-spline curve can be realized.

[0174] To enable the B-spline curve to be applied to the cornering trajectory, a quasi-uniform B-spline curve is used, and the first two control points are made to coincide by using multiple nodes.

[0175] Finally, each control point The coordinates and degree of the curve By inputting the data into simulation software, drawing and connecting each point, the nth-degree B-spline curve can be completely determined.

[0176] The vehicle body selected in this invention is TRACER MINI. It is assumed that the vehicle needs to complete a 90-degree turn within a 2000mm*2000mm space when cornering. The deviations generated when the AGV vehicle with differential chassis is working under the conditions of right-angle bend, arc, Bezier curve and B-spline curve are compared. An example is given to illustrate the optimal curve when the vehicle is cornering to a certain extent.

[0177] When a car is cornering on a path such as an S-shape, the deviation at the end of the corner will affect the direction and orientation of the trajectory of the next straight section. Therefore, the optimal curve should be one where the maximum deviation of the car during the cornering is not too large and the deviation at the end of the cornering is as small as possible. To this end, this invention sets a maximum deviation threshold for the car during the cornering.

[0178] Since the content studied in this invention is based on a kinematic model, acceleration is not considered, and a car model with uniform motion is adopted. Under the above premise, the deviations generated by the car when turning are compared by using predetermined trajectories of right-angle bends, circular arcs, Bézier curves and B-spline curves respectively.

[0179] First, when the predetermined trajectory is an ideal right-angle bend, i.e., a right-angle bend where the intersection point of perpendicular lines AB and BC is point B, the car moves in a straight line from point A to point B, then turns 90 degrees at point B, and then moves in a straight line from B to C. Since the predicted trajectory is a right-angle bend, the radius of curvature of the car at point B is 0, and the car maintains a constant speed. Therefore, the angular velocity at the inflection point is positive infinity, meaning the rotational speeds of the left and right drive wheels are also positive and negative infinity, respectively. Substituting the relationship between the DC motor speed and armature voltage, we can obtain that the armature voltage difference between the left and right drive wheel motors is positive infinity. Substituting this into the car's control model, we can see that when the input signal... When a singularity occurs, the output shows the path deviation of the car. A singularity may also occur, that is, a situation where the deviation is infinitely large. It can be seen that when the car is on a right-angle turn in the predetermined trajectory, it will have an infinitely large path deviation when it reaches point B.

[0180] In actual operation, when the AGV passes through a right-angle bend at a constant speed, it needs to change direction in a very short time at the turning point. This will cause the AGV to slide sideways and even lose control. This will not only affect the stability and safety of the AGV's movement, but also have a negative impact on the efficiency and accuracy of logistics and distribution. Therefore, it is not appropriate to use a right-angle bend as the predetermined trajectory for the AGV to turn.

[0181] When the predetermined trajectory is an arc, to complete a 90-degree turn within a 2000mm*2000mm space, the radius of the arc is 2000mm, meaning the curvature of the predetermined trajectory remains constant. Since the car's speed is constant, the kinematic model of the car shows that the angular velocity, the speed of the left and right drive wheels, and the rotational speed of the left and right drive wheels are all constant. According to the trajectory deviation calculation model, the maximum deviation of the car's operation is 1.238mm. After completing the turn, the deviation when the car enters the straight section is 1.232mm. It can be seen that the maximum deviation of the car's operation is very small, but the deviation is larger when entering the straight section after completing the turn.

[0182] When the predetermined trajectory is a Bézier curve, it is known that the shape of the Bézier curve is determined by the coordinates and number of control points. The order of the curve has a strict relationship with the number of control points, that is, the order equals the number of control points minus one. To achieve a 90-degree vehicle turn within a 2000mm*2000mm space, at least three control points are required, meaning the order of the Bézier curve must be at least second-order. Generally, the higher the order of the Bézier curve, the higher the fitting accuracy, but the computational load also increases. Therefore, a trade-off must be struck between accuracy and computational efficiency. In practical applications, for 90-degree vehicle turns, third- to fifth-order Bézier curves can meet the needs of most scenarios. Therefore, this invention uses Bézier curves with four and six control points respectively, i.e., third-order and fifth-order Bézier curves.

[0183] On the other hand, regarding the selection of control point coordinates, in order to ensure the smoothness and stability of path planning, the spacing between control points needs to be as uniform as possible. In addition, in order to make the movement trajectory of the vehicle more accurate, thereby improving the positioning accuracy and motion control accuracy of the vehicle, and improving the efficiency and accuracy of logistics distribution, the position of the control points should be as close as possible to the actual path. Finally, adopting a predetermined trajectory that is symmetrical about the axis of symmetry passing through the inflection point can also reduce the deviation during operation to a certain extent.

[0184] Then the control point is , , and Substituting the third-order Bézier curve into the trajectory deviation calculation model, we can obtain that the maximum deviation of the car's operation is 1.364mm, and the deviation when the car enters the straight section after completing the curve is 1.012mm.

[0185] Set the control point as , , , , and Substituting the fifth-order Bézier curve into the trajectory deviation calculation model, we can obtain that the maximum deviation of the car's operation is 1.738mm, and the deviation of the car when entering the straight section after completing the curve is 0.304mm.

[0186] Compared to the third-order Bézier curve, the maximum deviation of the car during operation is slightly larger, but the deviation is significantly reduced when entering the straight section after the turn. According to the standard of optimal curve, that is, the maximum deviation of the car when turning should not be greater than the set threshold, and the curve with the smallest deviation at the end of the turn is the curve that is more in line with the standard. The fifth-order Bézier curve is more in line with the standard.

[0187] When the predetermined trajectory is a B-spline curve, it is known that, similar to a Bézier curve, to achieve... To complete a 90-degree vehicle turn within a given space, at least three control points are required. Generally, a higher degree B-spline curve yields higher fitting accuracy, but also increases computational complexity. Therefore, a trade-off must be struck between accuracy and computational efficiency. In practical applications, using a third to fifth degree B-spline curve can meet the needs of most scenarios for a 90-degree vehicle turn. Furthermore, during actual fitting, if the number of control points is less than or equal to the degree of the B-spline curve, the computational load increases exponentially. Therefore, for a 90-degree vehicle turn, the number of control points is typically four to six. In summary, this invention uses third- and fifth-degree B-spline curves with four and six control points respectively to achieve the maximum fitting accuracy for the given degree.

[0188] Unlike Bézier curves, B-spline curves are not solely determined by control points. Moving or adding / removing control points does not cause significant changes to the entire curve. This means that the selection of control points does not necessarily adhere strictly to the three requirements of uniform control point spacing, control points closely conforming to the actual path, and control points being symmetrical about the axis of symmetry passing through inflection points. Instead, they can be selected based on the actual working requirements of the vehicle. Given that the vehicle needs to minimize path deviation when completing a turn, thus ensuring that the position and direction of the vehicle entering the next straight segment are as close to the requirements as possible, to reduce path deviation when the vehicle completes a turn, the control points should conform to the straight line as much as possible. This ensures that the curvature of the predetermined trajectory is not too large in the latter half of the turn, ultimately achieving the smallest possible path deviation at the end of the turn.

[0189] The cubic B-spline curve used in this invention employs the same control points as the third-order Bézier curve. This is because, for the four control points of a cubic B-spline curve, excluding the first and last two control points, the two middle control points are on a straight line. The above will result in only one control point in the first half of the trajectory when cornering, which will make the curvature of the first half too large. This will cause the maximum deviation during operation to not meet the requirements. Substituting the third-order B-spline curve into the trajectory deviation calculation model, we can find that the maximum deviation of the car is 1.362mm. After cornering, the deviation when entering the straight section is 0.989mm. Compared with the third-order Bézier curve, it can be seen that the maximum deviation is basically the same, and the deviation at the end of the cornering is slightly reduced.

[0190] Due to the local variability of B-spline curves, control points can be designed based on the actual working requirements of the trolley. Furthermore, the quintic B-spline curve selected in this invention has six control points. By excluding the first and last two definite control points, as many control points as possible can be made to closely approximate a straight line. Therefore, this invention selects two control points (excluding the first and last control points) on a straight line. Above, one control point, excluding the first and last control points, is on the y-axis, and the remaining control point is used as a transition point to prevent the curvature of the first half of the curve from being too large. Then, the deviation of the car during operation is slightly reduced by using symmetry, and integer coordinate values ​​are selected to reduce the amount of calculation.

[0191] Then set the control point to , , , , , Substituting the fifth-order B-spline curve into the trajectory deviation calculation model, we can obtain that the maximum deviation of the car's operation is 1.565mm, and the deviation when entering the straight section after the curve is 0.029mm.

[0192] Comparing the B-spline curves before and after the improvement, it can be seen that the maximum deviation of the improved B-spline curve is reduced, and the deviation at the end of the curve is significantly reduced. Compared with the cubic B-spline curve, the maximum deviation increases, but the deviation at the end of the curve is significantly reduced. Compared with the fifth-order Bézier curve, the maximum deviation increases, but the deviation at the end of the curve is significantly reduced. According to the criteria for optimal curves, when the improved fifth-order B-spline curve is used as the predetermined trajectory for cornering, the maximum deviation of the car during cornering is moderate, and the deviation at the end of the curve is significantly lower than that of the predetermined trajectories of circular arc, cubic Bézier curve, fifth-order Bézier curve, cubic B-spline curve, and the original fifth-order B-spline curve.

[0193] In summary, the optimal predetermined trajectory for cornering is a quintic B-spline curve where the control points are as close as possible to the extension of the next straight section of the trajectory after the curve, and the spacing between the control points in the first half of the curve is not too large. This minimizes the impact on the direction and orientation of the next straight section of the trajectory.

[0194] The above examples of the present invention are merely illustrative of the computational model and process of the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is impossible to exhaustively list all possible implementations here. Any obvious variations or modifications derived from the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A method for calculating the cornering trajectory deviation of an AGV (Automated Guided Vehicle) trolley based on a differential chassis, characterized in that, The method specifically includes the following steps: Step 1: Establish the kinematic model of the AGV with a differential chassis, and then establish the control model of the AGV based on the kinematic model of the AGV. Step 2: Control the AGV based on its control model and PID controller, and calculate the trajectory deviation based on the AGV's motion trajectory and the preset trajectory. Step 3: Select the cornering trajectory based on the maximum deviation during cornering and the deviation at the end of cornering for each preset trajectory.

2. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 1, characterized in that, The process of establishing the kinematic model is as follows: Coordinates of the center of the car satisfy: (2) in, This indicates the position of the car's center in the global coordinate system. Axial coordinates, This indicates the position of the car's center in the global coordinate system. Axial coordinates, express The first derivative, express The first derivative, This indicates the direction of the car's movement relative to the global coordinate system. Angle along the axial direction; According to equation (2), we get: (3) in, This represents the linear velocity at the center of the car. This represents the angular velocity at the center of the car. express The first derivative; The linear velocities of the left and right drive wheels are respectively: (4) in, This represents the radius of curvature of the instantaneous trajectory of the car. This indicates the linear velocity of the left drive wheel. This indicates the linear velocity of the right drive wheel. Indicates intermediate variables; Convert the speeds of the left and right drive wheels into the angular velocity of the trolley's center: (5) Convert the speeds of the left and right drive wheels into the linear velocity of the vehicle's center: (6) For the inverse kinematics problem of the trolley, according to equations (5) and (6), the velocities of the left and right driving wheels of the trolley can be expressed by the linear velocity and angular velocity of the center of the trolley as follows: (7) Combining formulas (3) and (7), the kinematic model of the AGV is obtained as follows: (8) in, This indicates the initial azimuth angle of the vehicle. This represents the initial position vector of the vehicle. Indicates time; The discretized state equation of the AGV is: (10) in, This indicates that the left drive wheel of the AGV is in The speed of time This indicates that the right drive wheel of the AGV is in The speed of time This represents the time interval between two adjacent moments. Indicates that the AGV cart is in Azimuth at time, This indicates that the center of the AGV is at... Location at any given moment This indicates that the center of the AGV is at... Location at any given moment Indicates that the AGV cart is in The azimuth at any given time.

3. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 2, characterized in that, The intermediate variable for: (1) in, Indicates the left drive wheel in the global coordinate system Position vector in the plane Indicates the right drive wheel in the global coordinate system Position vector in the plane.

4. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 3, characterized in that, The control model of the AGV includes an inverse kinematics control model and a forward kinematics control model.

5. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 4, characterized in that, The process of establishing the inverse kinematics control model is as follows: Calculate the linear velocity and angular velocity of the AGV center based on the known trajectory and formula (3), and then substitute the linear velocity and angular velocity of the AGV center into formula (7) to obtain the velocity of the left drive wheel and the velocity of the right drive wheel. Based on the speed of the left drive wheel, the functional relationship between the rotational speed of the left drive wheel and time can be obtained; similarly, based on the speed of the right drive wheel, the functional relationship between the rotational speed of the right drive wheel and time can be obtained. (11) in, express The rotational speed of the left drive wheel at any given moment. express The rotational speed of the right drive wheel at any given moment. Indicates the radius of the wheel; The functional relationship between the input armature terminal voltage and the rotational speed is obtained through analysis of the DC motor: (12) in, This represents the input armature terminal voltage. Indicates rotational speed; Substituting the functional relationship between rotational speed and time into equation (12), we obtain the functional relationship between the input armature terminal voltage and time, thus obtaining the inverse kinematics control model of the trolley.

6. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 5, characterized in that, The process of establishing the positive kinematic control model is as follows: The armature voltage of the motor is denoted as The electromechanical time constant is denoted as Then the relationship between the rotational speed of the drive wheel driven by the DC motor and the armature terminal voltage is: (13) in, Indicates the rotational speed of the drive wheel. Indicates the armature terminal voltage. Indicates gain. Represents the Laplace variable; Substituting the rotational speed calculated by equation (13) into equation (11) yields the speeds of the left and right driving wheels. Substituting the speeds of the left and right driving wheels into equation (10) yields the trajectory of the vehicle, thus obtaining the forward kinematic control model of the vehicle.

7. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 6, characterized in that, The gain for: (14) in, This represents the torque constant of the motor. This represents the electromotive force constant of the motor.

8. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 7, characterized in that, The control of the AGV based on its control model and PID controller is as follows: Step 2:

1. Use the output of the second PID controller as the input of the amplifier, then substitute the amplifier's output into the forward kinematics control model to obtain the speed of the driving wheel. Divide the speed of the driving wheel by... The result of the division is recorded as Then divide the result Multiply The result of the multiplication is denoted as Then multiply the result and Multiply them to get the final result. ; Step 22: Calculate the correction signal based on the actual trajectory of the AGV and the inverse kinematics control model. ; Steps two and three: The calibration signal The final multiplication result The difference is calculated, and the result is used as the input to the first PID controller. Step 24: Subtract the output of the first PID controller The difference result is used as the input of the second PID controller, and the process returns to step two-one to achieve closed-loop control of the AGV.

9. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 8, characterized in that, The preset trajectory includes circular arc curves, third-order Bézier curves, fifth-order Bézier curves, third-order B-spline curves, and fifth-order B-spline curves.

10. The method for calculating the cornering trajectory deviation of an AGV trolley oriented towards a differential chassis according to claim 9, characterized in that, The selection of the cornering trajectory based on the maximum deviation during cornering and the deviation at the end of cornering for each preset trajectory is specifically as follows: To set the maximum deviation threshold when cornering, first select curves whose maximum deviation when cornering is less than the set threshold, then compare the deviations at the end of the cornering of each selected preset curve, and select the preset trajectory with the smallest deviation at the end of the cornering.

Images