AGV automatic flexible docking method and system based on force-position fusion control

Abstract

The invention discloses an AGV automatic flexible docking method and system based on force and position fusion control. According to the method, firstly, primary pose alignment of a master AGV and a slave AGV is achieved through a laser tracker, then filtering and noise reduction are carried out on data of a fixedly-installed six-dimensional force sensor through Kalman filtering and other algorithms so as to accurately extract contact force information, and real-time vehicle body pose calculation is carried out based on the filtered force sensor data and the known vehicle body structure size. A force-position automatic compliant butt joint controller integrating an impedance control idea is further designed, and the controller dynamically adjusts the motion track of the slave vehicle by converting a force error into a position and posture compensation amount, so that the slave vehicle can actively and compliantly adapt to the position and posture deviation of the master vehicle after being contacted with the force-position automatic compliant butt joint controller. According to the method, the technical problems that in the AGV multi-vehicle linkage process, due to inaccurate single-position navigation butt joint, the system internal force is too large, the butt joint precision is insufficient, and repeated butt joint is needed can be effectively solved.

Description

Technical Field

[0001] This invention belongs to the field of automated guided vehicle (AGV) technology, specifically an automatic compliant docking method and system for AGVs based on force-position fusion control. Background Technology

[0002] With the rapid development of logistics automation and flexible manufacturing systems, Automated Guided Vehicles (AGVs) are playing an increasingly important role in material handling, production line docking, and other scenarios. Among these, the high-precision, non-destructive automatic docking of AGVs with fixed workstations, shelves, or other mobile platforms (i.e., master vehicles) is a key link in achieving full-process automation.

[0003] Currently, the mainstream technology for achieving automatic AGV docking relies primarily on absolute position sensing technologies, such as laser navigation, visual recognition, and QR code positioning. These technologies guide the AGV to the target location by constructing an offline global map or recognizing preset markers. However, this pure position control method has inherent limitations: First, its docking accuracy heavily depends on the measurement accuracy of the sensors themselves and the stability of the external environment. Interference such as visual obstruction, changes in lighting, or ground vibrations can easily lead to positioning errors, causing docking failure. Second, and more importantly, this is an "open-loop" docking strategy: once the AGV moves to the target point according to the position command, its control system considers the docking complete, failing to detect or respond to minute misalignments, jamming, or contact stresses that may occur between mechanical mechanisms during the docking process. This "hard contact" not only generates impact and noise, leading to mechanical wear, but in scenarios requiring extremely high docking precision (such as precision component insertion or charging interface connection), even a slight error can prevent success or cause equipment damage.

[0004] To overcome the shortcomings of pure position control, the industry has attempted to introduce force sensors to detect contact information. However, existing force control methods are mostly limited to simple threshold judgments, such as stopping abruptly or triggering an alarm after detecting that the contact force exceeds a certain safety threshold. While this can play a protective role, it fails to actively and smoothly utilize force information to guide the AGV to complete the final correction and docking, and cannot achieve true "intelligent compliance".

[0005] Therefore, existing technologies lack a control method that can organically integrate global position guidance with local force feedback, enabling AGVs to have "tactile" and "compliance" at the docking end, like skilled workers, and to adaptively eliminate docking stress and compensate for positional deviations, thereby achieving efficient, reliable, and non-destructive precise docking. Summary of the Invention

[0006] The purpose of this invention is to provide an automatic compliant docking method for AGVs based on force-position fusion control, in order to solve the problems of small misalignment, jamming, and failure to sense and respond to contact stress caused by insufficient navigation accuracy when AGVs are rigidly connected, resulting in equipment damage.

[0007] The technical solution to achieve the objective of this invention is as follows: On the one hand, an automatic compliant docking method for AGVs based on force-position fusion control is provided, the method comprising the following steps:

[0008] Step 1: The initial pose alignment between the slave vehicle and the master vehicle is achieved based on laser navigation, at which point the docking mechanism between the two achieves initial docking;

[0009] Step 2: Collect the contact force data between the master vehicle and the slave vehicle during the initial docking in Step 1;

[0010] Step 3: Perform Kalman filtering on the collected contact force data;

[0011] Step 4: Based on the filtered contact force data and vehicle dimensions, calculate the resultant force and resultant torque at the vehicle's center of gravity.

[0012] Step 5: Design a force-position fusion controller based on the resultant force and torque to convert force error into pose compensation;

[0013] Step 6: Based on the pose compensation, the adjusted pose of the slave vehicle is obtained according to the pose compensation algorithm, and then the slave AGV chassis is controlled to adjust the pose, thereby realizing a smooth connection between the slave vehicle and the master vehicle.

[0014] Furthermore, in step 2, contact force data is collected by force sensors installed on the slave vehicle. The contact force data includes force data in the x, y, and z axes; wherein the x-axis is along the travel direction of the master vehicle or slave vehicle, the y-axis is along the slave vehicle pointing towards the master vehicle, and the z-axis is along the vertical direction.

[0015] Furthermore, at least two force sensors are provided, which are respectively installed on the front differential wheel and the rear differential wheel of the vehicle, and all six-dimensional force sensors are located on the same side of the vehicle.

[0016] Furthermore, in step 3, the formula for Kalman filtering the collected contact force data is:

[0017]

[0018] In the formula, This represents the predicted state at time k before the data from time k is fused. This represents the predicted state at time k-1 before the data from time k-1 observations are fused. This is the optimal state estimate at time k-1; The error covariance matrix is ​​the prior estimate; This is the error covariance matrix of the optimal estimate at the previous time step, i.e., time step k-1; Let $\mathbf{k}$ be the error covariance matrix of the optimal estimate at the previous time step, i.e., time step $k$. The process noise covariance matrix; Kalman gain; To observe the noise covariance matrix; This represents the optimal state estimate at time k after incorporating the observed data. The actual observed value at time k; Here is the updated error covariance matrix; I is the identity matrix; This is the state transition matrix; To control the input matrix; The control quantity at time k; This is the observation matrix.

[0019] Furthermore, in step 5, pose compensation is obtained from the resultant force and resultant torque at the vehicle's center of gravity through an impedance controller, where the formula for the impedance controller is:

[0020]

[0021] In the formula, For the desired position of the vehicle With actual pose deviation, The contact force / torque vector measured by the force sensor. For the desired contact force / torque, Data is collected by the force sensor; , and These are the desired inertia, damping, and stiffness parameter matrices, respectively; These are acceleration deviation, velocity deviation, and pose deviation, respectively.

[0022] Furthermore, step 6 specifically includes:

[0023] Integrating the pose compensation yields the vehicle's speed and rotational speed commands;

[0024] The speed and rotation speed commands of the vehicle are decomposed into the speed and rotation speed of the four differential wheels of the vehicle;

[0025] The speed and rotation speed of the four differential wheels of the slave vehicle are input into the AGV wheel drive system, which controls the slave AGV chassis to adjust its position and posture to achieve smooth and automatic docking with the master vehicle.

[0026] On the other hand, an automatic compliant docking system for AGVs based on force-position fusion control is provided, the system comprising:

[0027] The first module is used to achieve: the initial pose alignment between the slave vehicle and the master vehicle based on laser navigation, at which time the docking mechanism between the two achieves initial docking;

[0028] The second module is used to collect contact force data between the master vehicle and the slave vehicle during the initial docking process achieved by the first module.

[0029] The third module is used to perform Kalman filtering on the collected contact force data;

[0030] The fourth module is used to calculate the resultant force and resultant moment at the vehicle's center of gravity based on the filtered contact force data and the vehicle's dimensions.

[0031] The fifth module is used to implement: design a force-position fusion controller based on the resultant force and torque, and convert force error into pose compensation;

[0032] The sixth module is used to: based on the pose compensation, obtain the adjusted pose of the slave vehicle according to the pose compensation algorithm, control the slave AGV chassis to adjust the pose, thereby realizing a smooth connection between the slave vehicle and the master vehicle.

[0033] Compared with the prior art, the significant advantages of this invention are:

[0034] (1) The method of this invention includes initial docking between the slave vehicle and the master vehicle based on laser navigation, real-time detection of forces in the three directions of x, y, and z axes by force sensors, and design of a Kalman filter algorithm to filter the collected forces in the three directions of the force sensors to obtain accurate data. Based on the filtered force sensor data and the vehicle body dimensions, the forces and torques at the connection point are converted to the center of gravity of the vehicle body to obtain the resultant force and resultant torque at the center of gravity. A compliant docking force-position impedance control method is designed to convert the forces on the vehicle body into an adjustment posture. The target adjustment posture is substituted into the AGV wheel drive system, and the chassis motor servo drive device is controlled based on control commands to collaboratively complete the specified actions, thereby realizing an active compliant hard connection between the slave vehicle and the master vehicle. This invention can enable the AGV to adaptively eliminate docking stress and compensate for posture deviations, thereby achieving efficient, reliable, and non-destructive accurate docking.

[0035] (2) Unlike the laser tracker and SLAM navigation docking method that are currently widely used, this invention uses a force-position control method to achieve flexible docking.

[0036] (3) More accurate force sensor data can be obtained through Kalman filtering, thereby improving the accuracy of force control.

[0037] (4) Compliant docking can be achieved by designing an impedance compliance control controller.

[0038] The present invention will now be described in further detail with reference to the accompanying drawings. Attached Figure Description

[0039] Figure 1 A schematic diagram of an AGV automatic compliant docking method based on force-position fusion control is provided for one embodiment.

[0040] Figure 2 A schematic diagram of the impedance control process provided for one embodiment. Detailed Implementation

[0041] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0042] It should be noted that if the embodiments of the present invention involve descriptions such as "first" and "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined with "first" and "second" may explicitly or implicitly include at least one of those features. Furthermore, the technical solutions of the various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0043] With the rapid development of logistics automation and flexible manufacturing systems, Automated Guided Vehicles (AGVs) are playing an increasingly important role in material handling, production line docking, and other scenarios. This also presents a demand for multi-scenario applications. To move larger, heavier objects, it is often necessary to dock several AGVs in a coordinated manner. However, current docking methods based on laser trackers and SLAM navigation suffer from accuracy issues, which may lead to excessive internal forces and damage to the docking mechanism during docking. Therefore, this application proposes an automatic compliant docking method for AGVs based on force-position fusion control to achieve flexible docking.

[0044] In one embodiment, combined Figure 1 This paper provides an automatic compliant docking method for AGVs based on force-position fusion control, the method comprising the following steps:

[0045] Step 1: The initial pose alignment between the slave vehicle and the master vehicle is achieved based on laser navigation, at which point the docking mechanism between the two achieves initial docking;

[0046] Step 2: Collect the contact force data between the master vehicle and the slave vehicle during the initial docking in Step 1;

[0047] Step 3: Perform Kalman filtering on the collected contact force data to remove noise and high-frequency components, and obtain a more accurate force signal.

[0048] Step 4: Based on the filtered contact force data and vehicle dimensions, calculate the resultant force and resultant torque at the vehicle's center of gravity.

[0049] Step 5: Design a force-position fusion controller based on the resultant force and torque to convert force error into pose compensation;

[0050] Step 6: Based on the pose compensation, the adjusted pose of the slave vehicle is obtained according to the pose compensation algorithm, and then the slave AGV chassis is controlled to adjust the pose, thereby realizing a smooth connection between the slave vehicle and the master vehicle.

[0051] Furthermore, in one embodiment, in step 2, contact force data is collected by a six-dimensional force sensor installed on the slave vehicle. The contact force data includes force data in the x, y, and z axes; wherein the x-axis is along the driving direction of the master vehicle or slave vehicle, the y-axis is along the slave vehicle pointing towards the master vehicle, and the z-axis is along the vertical direction.

[0052] Preferably, in some embodiments, at least two six-dimensional force sensors are provided, respectively installed on the front differential wheel and the rear differential wheel of the vehicle, and all six-dimensional force sensors are located on the same side of the vehicle.

[0053] Furthermore, in one embodiment, in step 3, the formula for performing Kalman filtering on the collected contact force data is:

[0054]

[0055] In the formula, This represents the predicted state at time k before the data from time k is fused. This represents the predicted state at time k-1 before the data from time k-1 observations are fused. This is the optimal state estimate at time k-1; The error covariance matrix is ​​the prior estimate; This is the error covariance matrix of the optimal estimate at the previous time step, i.e., time step k-1; Let $\mathbf{k}$ be the error covariance matrix of the optimal estimate at the previous time step, i.e., time step $k$. The process noise covariance matrix; Kalman gain; To observe the noise covariance matrix; This represents the optimal state estimate at time k after incorporating the observed data. The actual observed value at time k; Here is the updated error covariance matrix; I is the identity matrix; This is the state transition matrix; To control the input matrix; The control quantity at time k; This is the observation matrix.

[0056] Furthermore, in one embodiment, in step 5, pose compensation is obtained from the resultant force and resultant torque at the vehicle's center of gravity through an impedance compliance controller, wherein the formula for the impedance compliance controller is:

[0057]

[0058] In the formula, For the desired position of the vehicle With actual pose deviation, The contact force / torque vector measured by the force sensor. For the desired contact force / torque, Data is collected by the force sensor; , and These are the desired inertia, damping, and stiffness parameter matrices, respectively; These are acceleration deviation, velocity deviation, and pose deviation, respectively.

[0059] Furthermore, in one embodiment, combined with Figure 2 Step 6 specifically includes:

[0060] Integrating the pose compensation yields the vehicle's speed and rotational speed commands;

[0061] The speed and rotation speed commands of the vehicle are decomposed into the speed and rotation speed of the four differential wheels of the vehicle;

[0062] The speed and rotation speed of the four differential wheels of the slave vehicle are input into the AGV wheel drive system, which controls the slave AGV chassis to adjust its position and posture to achieve smooth and automatic docking with the master vehicle.

[0063] In one embodiment, an AGV automatic compliant docking system based on force-position fusion control is provided, the system comprising:

[0064] The first module is used to achieve: the initial pose alignment between the slave vehicle and the master vehicle based on laser navigation, at which time the docking mechanism between the two achieves initial docking;

[0065] The second module is used to collect contact force data between the master vehicle and the slave vehicle during the initial docking process achieved by the first module.

[0066] The third module is used to perform Kalman filtering on the collected contact force data;

[0067] The fourth module is used to calculate the resultant force and resultant moment at the vehicle's center of gravity based on the filtered contact force data and the vehicle's dimensions.

[0068] The fifth module is used to implement: design a force-position fusion controller based on the resultant force and torque, and convert force error into pose compensation;

[0069] The sixth module is used to: based on the pose compensation, obtain the adjusted pose of the slave vehicle according to the pose compensation algorithm, control the slave AGV chassis to adjust the pose, thereby realizing a smooth connection between the slave vehicle and the master vehicle.

[0070] Specific limitations regarding the AGV automatic compliant docking system based on force-position fusion control can be found in the limitations of the AGV automatic compliant docking method based on force-position fusion control mentioned above, and will not be repeated here. Each module in the aforementioned AGV automatic compliant docking system based on force-position fusion control can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the corresponding operations of each module.

[0071] In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the functions of each module of the AGV automatic compliant docking system based on force-position fusion control.

[0072] For specific limitations on each module, please refer to the limitations on the AGV automatic compliant docking method based on force-position fusion control mentioned above, which will not be repeated here.

[0073] In one embodiment, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the functions of each module of the AGV automatic compliant docking system based on force-position fusion control.

[0074] For specific limitations on each module, please refer to the limitations on the AGV automatic compliant docking method based on force-position fusion control mentioned above, which will not be repeated here.

[0075] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention without departing from its spirit and scope should be included within the protection scope of the present invention.

Claims

1. An automatic compliant docking method for AGVs based on force-position fusion control, characterized in that, The method includes the following steps: Step 1: The initial pose alignment between the slave vehicle and the master vehicle is achieved based on laser navigation, at which point the docking mechanism between the two achieves initial docking; Step 2: Collect the contact force data between the master vehicle and the slave vehicle during the initial docking in Step 1; Step 3: Perform Kalman filtering on the collected contact force data; Step 4: Based on the filtered contact force data and vehicle dimensions, calculate the resultant force and resultant torque at the vehicle's center of gravity. Step 5: Design a force-position fusion controller based on the resultant force and torque to convert force error into pose compensation; Step 6: Based on the pose compensation, the adjusted pose of the slave vehicle is obtained according to the pose compensation algorithm, and then the slave AGV chassis is controlled to adjust the pose, thereby realizing a smooth connection between the slave vehicle and the master vehicle.

2. The AGV automatic compliant docking method based on force-position fusion control according to claim 1, characterized in that, In step 2, contact force data is collected by force sensors installed on the vehicle. The contact force data includes force data in the x, y, and z axes. The x-axis is along the direction of travel of the main vehicle or the slave vehicle, the y-axis is along the slave vehicle pointing towards the main vehicle, and the z-axis is in the vertical direction.

3. The AGV automatic compliant docking method based on force-position fusion control according to claim 2, characterized in that, At least two force sensors are provided, which are respectively installed on the front differential wheel and the rear differential wheel of the vehicle, and all six-dimensional force sensors are located on the same side of the vehicle.

4. The AGV automatic compliant docking method based on force-position fusion control according to claim 1, characterized in that, In step 3, the formula for Kalman filtering the collected contact force data is: ； In the formula, This represents the predicted state at time k before the data from time k is fused. This represents the predicted state at time k-1 before the data from time k-1 observations are fused. This is the optimal state estimate at time k-1; The error covariance matrix is ​​the prior estimate; This is the error covariance matrix of the optimal estimate at the previous time step, i.e., time step k-1; This is the error covariance matrix of the optimal estimate at the previous time step, i.e., time k. The process noise covariance matrix; Kalman gain; To observe the noise covariance matrix; This represents the optimal state estimate at time k after incorporating the observed data. The actual observed value at time k; This is the updated error covariance matrix; I is the identity matrix; This is the state transition matrix; To control the input matrix; The control quantity at time k; This is the observation matrix.

5. The AGV automatic compliant docking method based on force-position fusion control according to claim 2, characterized in that, In step 5, the resultant force and resultant torque at the vehicle's center of gravity are used to obtain pose compensation through an impedance controller, where the formula for the impedance controller is: ； In the formula, To the desired position of the vehicle With actual pose deviation, The contact force / torque vector measured by the force sensor. For the desired contact force / torque, Data is collected by the force sensor; , and These are the desired inertia, damping, and stiffness parameter matrices, respectively. These are acceleration deviation, velocity deviation, and pose deviation, respectively.

6. The AGV automatic compliant docking method based on force-position fusion control according to claim 1, characterized in that, Step 6 specifically includes: Integrating the pose compensation yields the vehicle's speed and rotational speed commands; The speed and rotation speed commands of the vehicle are decomposed into the speed and rotation speed of the four differential wheels of the vehicle; The speed and rotation speed of the four differential wheels of the slave vehicle are input into the AGV wheel drive system, which controls the slave AGV chassis to adjust its position and posture to achieve smooth and automatic docking with the master vehicle.

7. The AGV automatic compliant docking system based on force-position fusion control according to any one of claims 1 to 6, characterized in that, The system includes: The first module is used to achieve: the initial pose alignment between the slave vehicle and the master vehicle based on laser navigation, at which time the docking mechanism between the two achieves initial docking; The second module is used to collect contact force data between the master vehicle and the slave vehicle during the initial docking process achieved by the first module. The third module is used to perform Kalman filtering on the collected contact force data; The fourth module is used to calculate the resultant force and resultant moment at the vehicle's center of gravity based on the filtered contact force data and the vehicle's dimensions. The fifth module is used to implement: design a force-position fusion controller based on the resultant force and torque, and convert force error into pose compensation; The sixth module is used to: based on the pose compensation, obtain the adjusted pose of the slave vehicle according to the pose compensation algorithm, control the slave AGV chassis to adjust the pose, thereby realizing a smooth connection between the slave vehicle and the master vehicle.

Images