AGV motion control method

Abstract

The invention discloses an AGV (Automatic Guided Vehicle) motion control method which comprises the following steps: (1) setting a control center o on an AGV, constructing a coordinate system by taking the control center o as an original point, and determining a forward reference point of the control center o at a current position; (2) constructing a state error equation from a forward reference point to a control center o, and determining controlled variables of the vehicle body in an O coordinate system, including angle speed and linear speed; and (3) converting the control quantity of the vehicle body in the O coordinate system into the wheel speed and the rotating angle of the steering wheel. A control center o is set on the AGV according to the requirement of an application scene, the AGV is controlled based on the control center o, and the control quantity of the AGV is converted to a steering wheel by calibrating a transformation matrix between a fixed control center p and the control center o in advance.

Description

Technical Field

[0001] This invention belongs to the field of AGV control technology, and more specifically, this invention relates to an AGV motion control method. Background Technology

[0002] Precise motion control of AGVs (Automated Guided Vehicles) is one of the core technologies for their efficient operation in industries such as manufacturing, warehousing, and logistics.

[0003] To ensure that AGVs can complete tasks accurately and along pre-defined paths, various technologies are required for motion control. The control center of the AGV chassis is influenced by the wheel structure and cannot be arbitrarily changed. In certain special scenarios, the existing control center p can introduce control errors and produce undesirable results. For example, the motion control center of a forklift is limited by the geometric relationship between the wheels, but the forklift's working area is at the fork teeth. Therefore, even a slight offset at the control center will inevitably cause fluctuations at the fork teeth, preventing them from meeting the corresponding operational requirements.

[0004] Furthermore, for composite robots with AGV chassis, if the coordinate system of the robotic arm and the motion control center p of the AGV chassis do not coincide, the accuracy of the robotic arm's end effector (such as grasping and assembly) will be affected. If the actual motion of the robotic arm's coordinate system fails to properly compensate for the movement of the AGV chassis, significant errors may occur in the positioning of grasping or assembly tasks, causing local errors to gradually accumulate and thus amplify the global error of the entire system. Summary of the Invention

[0005] The present invention provides an AGV motion control method, which aims to improve at least one of the above-mentioned problems.

[0006] This invention is implemented as follows: an AGV motion control method, the method is as follows:

[0007] (1) Specify control center o on the AGV, construct a coordinate system with control center o as the origin, and determine the forward reference point of control center o at the current position;

[0008] (2) Construct the state error equation from the forward reference point to the control center o, and determine the control quantities of the vehicle body in the O coordinate system, including: angular velocity and linear velocity.

[0009] (3) Convert the control quantity of the vehicle body in the O coordinate system into the wheel speed of the steering wheel. and corners

[0010] Furthermore, the method for determining the forward reference point of the control center o is as follows:

[0011] (11) Form a local reference path for the control center o at its current location;

[0012] (12) A set of reference points sampled on a local reference path;

[0013] (13) Select the forward reference point at the current speed from the reference points on the local reference path.

[0014] Furthermore, the method for forming the local reference path is as follows:

[0015] Calculate the Euclidean distance between each path point in the path point set and the control center o. Select the path point with the smallest Euclidean distance as the nearest path point. Find the m-1 path points behind the nearest path point from the path point set. Fit a local reference path based on the nearest path point and the m-1 path points behind it.

[0016] Furthermore, the specific method for determining reference points on the local reference path is as follows:

[0017] Extract the start and end points of the local reference path, calculate the length of the straight line containing the start and end points, and calculate the distance interval corresponding to sampling n reference points on the straight line. ;

[0018] Based on the start and end points, the system detects whether the local reference path is a straight path. If the detection result is yes, the x-coordinate of the reference point remains unchanged, and the y-coordinate is based on the interval. Update the algorithm; if the detection result is negative, calculate the coordinates of n reference points using the trapezoidal bisection method.

[0019] Furthermore, the method for determining the forward reference point is as follows:

[0020] Based on the current speed of the AGV The desired reference point is calculated using the control period T.

[0021] Based on the location of the control center o on the map, the coordinates of the n reference points on the local reference path are transformed to the O coordinate system. Then, the Euclidean distance between the n reference points on the local reference path and the desired reference point is calculated, and the reference point corresponding to the minimum Euclidean distance is taken as the forward reference point.

[0022] Furthermore, the state error equation from the forward reference point r to the control center point o on the local reference path is as follows:

[0023] ;

[0024] in, Represents linear velocity The angle between the x-axis and the x-axis in the R-coordinate system. , These represent the x-coordinate and y-coordinate of the control center o in the R coordinate system, respectively. Let be the slope of the tangent line at the forward reference point r.

[0025] Furthermore, the control quantity of the vehicle body in the O coordinate system Transform to the P coordinate system, and convert the control variables of the vehicle body in the P coordinate system. Converted to wheel speed and corners ;

[0026] In this system, the P coordinate system has its origin at the drive wheel control center p.

[0027] Furthermore, the kinematic model from control center p to control center o is as follows:

[0028] ;

[0029] in, Let O be the coordinates of the control center in the P coordinate system. Represents the angular velocity of the vehicle body about the z-axis in coordinate system O. , Let X and Y represent the linear velocities of the vehicle body in the x-axis and y-axis directions, respectively, in the O coordinate system. This represents the angular velocity of the vehicle body about the z-axis in the P coordinate system. This represents the linear velocity of the vehicle body along the x-axis in the P coordinate system.

[0030] Furthermore, the wheel speed of the steering wheel and corners The specific calculation formula is as follows:

[0031] ;

[0032] in, This represents the distance from the control center p to the center of the rear wheel. This represents the angular velocity of the vehicle body about the z-axis in the P coordinate system. This represents the linear velocity of the vehicle body along the x-axis in the P coordinate system.

[0033] According to the requirements of the application scenario, the present invention sets a control center o on the AGV, and controls the AGV based on the control center o. By pre-calibrating the transformation matrix between the fixed control center p and the control center o, the control quantity of the AGV is converted to the steering wheel. Attached Figure Description

[0034] Figure 1 This is a top view of a single-steering wheel structure forklift provided in an embodiment of the present invention;

[0035] Figure 2This is a schematic diagram illustrating the selection of the designated control center o for a single-steering wheel structure forklift provided in an embodiment of the present invention;

[0036] Figure 3 This is a kinematic model of a single-steering wheel structure forklift vehicle provided in an embodiment of the present invention;

[0037] Figure 4 This is a flowchart of the AGV motion control method provided in an embodiment of the present invention. Detailed Implementation

[0038] The specific embodiments of the present invention will be further described in detail below with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0039] Figure 1 This is a top view of a single-steering-wheel forklift. Figure 2 To select a control center point for a single-steering-wheel forklift, the wheel speed of the forklift's steering wheel is... The steering wheel's rotation angle is With the drive wheel control center p as the origin, the direction of gear extension as the x-axis, the axles of the two front wheels as the y-axis, and the z-axis perpendicular to the xy plane, a coordinate system P is established, from which we can obtain:

[0040] (1)

[0041] in, This represents the distance from the control center p to the center of the rear wheel. This represents the angular velocity of the vehicle body about the z-axis in the P coordinate system. This represents the linear velocity of the vehicle body along the x-axis in the P coordinate system.

[0042] Arbitrarily define a control center o on the AGV, with control center o as the origin, the extension direction of the gear teeth as the x-axis, the axles of the two front wheels as the y-axis, and the z-axis perpendicular to the xy plane. Establish the O coordinate system. The vector po between control center p and control center o in the P coordinate system is expressed as follows: That is, the coordinates of the control center o in the P coordinate system are Kinematic model of control center p to control center o

[0043] (2)

[0044] in, Represents the angular velocity of the vehicle body about the z-axis in coordinate system O. , These represent the linear velocities of the vehicle body in the x-axis and y-axis directions in the O coordinate system, respectively.

[0045] Figure 4The flowchart of the AGV motion control method provided in the embodiment of the present invention is as follows:

[0046] (1) Determine the forward reference point of the AGV's current position;

[0047] In this embodiment of the invention, the method for determining the forward reference point of the AGV's current position is as follows:

[0048] (11) Form a local reference path at the current location;

[0049] The planned path consists of a series of path points, which are stored in a path point set. The Euclidean distance between each path point in the path point set and the control center point o is calculated. The path point with the smallest Euclidean distance is taken as the nearest path point. The m-1 path points behind the nearest path point are found from the path point set. A local reference path is fitted based on the nearest path point and the m-1 path points behind it. A cubic curve equation is used to fit the local reference path.

[0050] (12) A set of reference points sampled on a local reference path;

[0051] In this embodiment of the invention, the method for determining reference points on a local reference path is as follows:

[0052] Extract the start and end points of the local reference path, calculate the length of the straight line containing the start and end points, and calculate the distance interval corresponding to sampling n reference points on the straight line. ;

[0053] Based on the start and end points, the system detects whether the local reference path is a straight path. If the detection result is yes, the x-coordinate of the reference point remains unchanged, and the y-coordinate is based on the interval. Update the algorithm; if the detection result is negative, calculate the coordinates of n reference points using the trapezoidal bisection method.

[0054] In this embodiment of the invention, if the number of path points behind the nearest path point in the path point set is less than 3, and the fitting of the local reference path cannot be completed, then the nearest path point is used as the forward reference point.

[0055] (13) Select the forward reference point at the current speed from the reference points on the local reference path.

[0056] In this embodiment of the invention, the method for determining the forward reference point is as follows:

[0057] (131) Based on the current speed of the AGV The desired reference point is calculated using the control period T, and the specific calculation formula is as follows:

[0058] (3)

[0059] in, , The coordinates of the nearest path point in coordinate system O. , Let O be the coordinates of the desired reference point in the O coordinate system. The external tangent angle of the circle representing the point on the nearest path.

[0060] (132) Based on the location of the control center o on the map, the coordinates of the n reference points in the local reference path are transformed to the O coordinate system. Then, the Euclidean distance between the n reference points in the local reference path and the desired reference point is calculated, and the reference point corresponding to the minimum Euclidean distance is taken as the forward reference point.

[0061] (2) Determine the control quantities of the vehicle body in the O coordinate system based on the state error equation from the forward reference point to the current control center O, including: angular velocity and linear velocity.

[0062] In this embodiment of the invention, an R coordinate system is established with the forward reference point r as the origin, the tangent direction of the reference point as the x-axis, the radial direction of the reference point as the y-axis, and the z-axis perpendicular to the xy plane. Figure 3 As shown, the state error equation from the forward reference point r to the control center point o on the local reference path is constructed as follows:

[0063] (4)

[0064] in, Represents linear velocity The angle between the x-axis and the x-axis in the R-coordinate system. , These represent the x-coordinate and y-coordinate of the control center o in the R coordinate system, respectively. Let be the slope of the tangent line at the forward reference point r.

[0065] A Taylor expansion is performed at the forward reference point to obtain the linearized state-space representation. The continuous-time linear model is then discretized using the Euler method. State feedback is designed to minimize the quadratic performance index, thereby obtaining the control input of the control center o. .

[0066] (3) Convert the control quantity of the vehicle body in the O coordinate system into the wheel speed of the steering wheel. and corners .

[0067] Based on formula (2), the control quantity of the vehicle body in the O coordinate system is... Transform to the P coordinate system, and based on formula (1), calculate the control quantity of the vehicle body in the P coordinate system. Converted to wheel speed and corners .

[0068] The present invention has been described by way of example. Obviously, the specific implementation of the present invention is not limited to the above-described manner. Any non-substantial improvements made using the inventive concept and technical solution of the present invention, or the direct application of the inventive concept and technical solution of the present invention to other occasions without modification, are all within the protection scope of the present invention.

Claims

1. An AGV motion control method, characterized in that, The method is as follows: (1) Set a control center o on the AGV, construct a coordinate system with the control center o as the origin, and determine the forward reference point of the control center o at the current position; (2) Construct the state error equation from the forward reference point to the control center o, and determine the control quantities of the vehicle body in the O coordinate system, including: angular velocity and linear velocity; (3) Convert the control quantity of the vehicle body in the O coordinate system into the wheel speed of the steering wheel. and corners .

2. The AGV motion control method as described in claim 1, characterized in that, The method for determining the forward reference point of control center o is as follows: (11) Form a local reference path for the control center o at its current location; (12) A set of reference points sampled on a local reference path; (13) Select the forward reference point at the current speed from the reference points on the local reference path.

3. The AGV motion control method as described in claim 2, characterized in that, The method for forming a local reference path is as follows: Calculate the Euclidean distance between each path point in the path point set and the control center o. Select the path point with the smallest Euclidean distance as the nearest path point. Find the m-1 path points behind the nearest path point from the path point set. Fit a local reference path based on the nearest path point and the m-1 path points behind it.

4. The AGV motion control method as described in claim 2, characterized in that, The specific method for determining reference points on a local reference path is as follows: Extract the start and end points of the local reference path, calculate the length of the straight line containing the start and end points, and calculate the distance interval corresponding to sampling n reference points on the straight line. ; Based on the start and end points, the system detects whether the local reference path is a straight path. If the detection result is yes, the x-coordinate of the reference point remains unchanged, and the y-coordinate is based on the interval. Update the algorithm; if the detection result is negative, calculate the coordinates of n reference points using the trapezoidal bisection method.

5. The AGV motion control method as described in claim 2, characterized in that, The method for determining the forward reference point is as follows: Based on the current speed of the AGV The desired reference point is calculated using the control period T. Based on the position of the control center point o on the map, the coordinates of the n reference points on the local reference path are transformed to the O coordinate system. Then, the Euclidean distance between the n reference points on the local reference path and the desired reference point is calculated, and the reference point corresponding to the minimum Euclidean distance is taken as the forward reference point.

6. The AGV motion control method as described in claim 2, characterized in that, The state error equation from the forward reference point r to the control center point o on the local reference path is as follows: ； in, Represents linear velocity The angle between the x-axis and the x-axis in the R-coordinate system. , These represent the x-coordinate and y-coordinate of the control center o in the R coordinate system, respectively. Let be the slope of the tangent line at the forward reference point r.

7. The AGV motion control method as described in claim 1, characterized in that, The control quantity of the vehicle body in the O coordinate system Transform to the P coordinate system, and convert the control variables of the vehicle body in the P coordinate system. Converted to wheel speed and corners ; In this system, the P coordinate system has its origin at the drive wheel control center p.

8. The AGV motion control method as described in claim 7, characterized in that, The kinematic model from control center p to control center o is as follows: ； in, Let O be the coordinates of the control center in the P coordinate system. Represents the angular velocity of the vehicle body about the z-axis in coordinate system O. , Let X and Y represent the linear velocities of the vehicle body in the x-axis and y-axis directions, respectively, in the O coordinate system. This represents the angular velocity of the vehicle body about the z-axis in the P coordinate system. This represents the linear velocity of the vehicle body along the x-axis in the P coordinate system.

9. The AGV motion control method as described in claim 7, characterized in that, Wheel speed of the steering wheel and corners The specific calculation formula is as follows: ； in, This represents the distance from the control center p to the center of the rear wheel. This represents the angular velocity of the vehicle body about the z-axis in the P coordinate system. This represents the linear velocity of the vehicle body along the x-axis in the P coordinate system.

Images