AGV reference surface positioning control method based on automation

Abstract

The invention discloses an AGV reference surface positioning control method based on automation, and particularly relates to the technical field of automatic guided vehicle control, and the method comprises the steps: firstly, judging whether an AGV enters a precise positioning activation area through a built-in positioning system of the AGV, and then carrying out the precise positioning of the AGV through fusing the data of a visual sensor, a laser radar and an inertial measurement unit, acquiring a real-time pose deviation of the AGV relative to the reference surface, planning a local path by using a dynamic window method, generating a motion control instruction, controlling the AGV to approach the reference surface, evaluating an approaching result, and confirming a positioning control result according to an evaluation result; an abnormal signal is fed back to the management terminal for man-machine interaction according to the positioning control result; through a staged strategy of global coarse positioning and local fine positioning, multiple sensors are fused, limitation and accumulative errors of a single sensor are effectively eliminated, and AGV positioning precision and accuracy are further improved.

Description

Technical Field

[0001] This invention relates to the field of automated guided vehicle control technology, specifically to an automated AGV reference plane positioning control method. Background Technology

[0002] Automated Guided Vehicles (AGVs) play a crucial role in modern intelligent warehousing and flexible manufacturing systems. One of their core performance indicators is the docking accuracy at target workstations (such as charging stations, loading and unloading ports, and assembly tables). These target workstations typically have a physical or virtual "reference surface," and the AGV needs to control the alignment error between its specific reference point and this reference surface to the millimeter level.

[0003] In existing technologies, the positioning control of AGVs mainly relies on dead reckoning methods: estimating position and heading by measuring wheel speed; local positioning methods based on QR codes or reflectors: AGVs obtain absolute position by reading pre-set QR codes on the ground or scanning reflectors at fixed positions; and SLAM positioning methods based on lidar or vision, thereby achieving the purpose of automated AGV reference plane positioning control.

[0004] Current AGV datum plane positioning control methods have many shortcomings. For example, in terms of positioning accuracy, traditional methods such as magnetic strip navigation and QR code navigation are limited by the accuracy of navigation markers and installation errors, making it difficult to meet high-precision positioning requirements. Regarding environmental adaptability, laser-guided AGVs are affected by laser beam reflection in environments with strong direct sunlight, dust, or uneven ground, leading to positioning deviations. Visual navigation AGVs are highly sensitive to changes in lighting conditions; in unstable lighting environments, the image information acquired by the camera will be distorted, affecting positioning accuracy. Furthermore, they lack autonomous decision-making and real-time adjustment capabilities when facing complex task scheduling and dynamic environmental changes. In summary, existing technologies cannot meet high-precision requirements and are susceptible to interference and have poor stability in the final approximation stage. Therefore, there is an urgent need in this field for an AGV datum plane positioning control method that can guarantee final positioning accuracy while possessing good flexibility and environmental adaptability. Summary of the Invention

[0005] In order to overcome the above-mentioned defects of the prior art, embodiments of the present invention provide an automated AGV reference plane positioning control method to solve the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention provides the following technical solution: an automated AGV reference plane positioning control method, comprising: S1: Through the AGV's built-in positioning system, a global path from the starting point to the target workstation located on the reference plane is planned, and the AGV is judged in real time whether it enters the fine positioning activation area, and the trigger signal for the AGV to enter the fine positioning activation area is obtained. S2: Receive the trigger signal for the AGV to enter the precision positioning activation area, and obtain the real-time pose deviation of the AGV relative to the reference plane by fusing data from the vision sensor, lidar and inertial measurement unit; S3: Based on the real-time pose deviation of the AGV relative to the reference plane, a local path is planned using the dynamic window method and motion control commands are generated to control the AGV to approach the reference plane, and the result of the AGV approaching the reference plane after the control commands are executed is obtained. S4: Evaluate the AGV approximation result after the control command is executed, confirm the positioning control result based on the evaluation result, and obtain the positioning control result abnormal signal; S5: Feedback is provided on abnormal signals of positioning control results, warnings are issued based on the feedback abnormal signals, and warning signals are sent to the management terminal for human-machine interaction.

[0007] The technical effects and advantages of this invention are as follows: 1. This invention adopts a phased strategy of global coarse positioning + local fine positioning, and combines extended Kalman filter algorithm to fuse multiple sensors such as vision sensor, lidar and IMU, which effectively eliminates the limitations and cumulative errors of single sensor, and provides stable and reliable pose feedback in the final approximation stage, thereby improving the positioning accuracy and precision of AGV. 2. This invention uses a dynamic window method to balance objectives such as heading alignment, obstacle avoidance, speed and efficiency in local path planning, enabling the AGV to quickly converge to the posture deviation range during the fine positioning stage; at the same time, based on the final confirmation step of the proximity sensor, it provides double insurance for successful positioning and ensures the absolute reliability of docking with downstream equipment in the automated process. 3. This invention provides feedback on abnormal signals in the positioning control results and issues early warnings based on these signals. It has a complete fault detection and recovery mechanism and can autonomously attempt recovery when subjected to severe interference or unexpected situations. This enhances the autonomous decision-making and real-time adjustment capabilities of the entire system, thereby helping to improve the accuracy of AGV reference plane positioning control. Attached Figure Description

[0008] Figure 1 This is a schematic diagram of the overall process of the present invention.

[0009] Figure 2 This is a schematic diagram of the method flow of the present invention.

[0010] Figure 3 This is a schematic diagram of the sensor fusion process of the present invention.

[0011] Figure 4 This is a schematic diagram of the process by which the AGV of the present invention approaches the reference plane. Detailed Implementation

[0012] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0013] Please see Figure 1 As shown, the present invention provides an automated AGV reference plane positioning control system, including an AGV reference plane initial positioning module, a multi-sensor fusion fine positioning module, an AGV reference plane positioning control module, an AGV reference plane positioning control confirmation module, and an AGV reference plane positioning control fault-tolerant module.

[0014] The AGV reference plane initial positioning module is connected to the multi-sensor fusion fine positioning module. The AGV reference plane positioning control module is connected to the AGV reference plane positioning control confirmation module and the multi-sensor fusion fine positioning module, respectively. The AGV reference plane positioning control fault-tolerant module is connected to the multi-sensor fusion fine positioning module, the AGV reference plane positioning control module, and the AGV reference plane positioning control confirmation module, respectively.

[0015] AGV datum plane initial positioning module: Through the AGV's built-in positioning system, it plans a global path from the starting point to the target workstation where the datum plane is located, and determines in real time whether the AGV has entered the fine positioning activation area. It then transmits the trigger signal for the AGV to enter the fine positioning activation area to the multi-sensor fusion fine positioning module. Multi-sensor fusion precision positioning module: Receives the trigger signal of the AGV entering the precision positioning activation area, obtains the real-time pose deviation of the AGV relative to the reference plane by fusing data from vision sensors, lidar and inertial measurement unit, and transmits it to the AGV reference plane positioning control module; AGV reference plane positioning control module: Based on the real-time pose deviation of the AGV relative to the reference plane, it uses the dynamic window method to plan a local path and generate motion control commands to control the AGV to approach the reference plane, and transmits the AGV approach result after the execution of the control commands to the AGV reference plane positioning control confirmation module. AGV reference plane positioning control confirmation module: evaluates the AGV approximation result after the control command is executed, confirms the positioning control result based on the evaluation result, and transmits the abnormal positioning control result signal to the AGV reference plane positioning control fault tolerance module. AGV reference plane positioning control fault-tolerant module: Feeds back abnormal signals of positioning control results, issues early warnings based on the feedback abnormal signals, and sends the early warning signals to the management terminal for human-machine interaction.

[0016] Please see Figure 2 As shown, an automated AGV reference plane positioning control method includes: S1: Using the AGV's built-in positioning system, a global path from the starting point to the target workstation where the reference plane is located is planned, and the AGV is judged in real time whether it enters the fine positioning activation area, and a trigger signal for the AGV to enter the fine positioning activation area is obtained; S2: Receiving the trigger signal for the AGV to enter the fine positioning activation area, and obtaining the real-time pose deviation of the AGV relative to the reference plane by fusing data from vision sensors, lidar, and inertial measurement units; S3: Based on the real-time pose deviation of the AGV relative to the reference plane, a local path is planned using the dynamic window method and motion control commands are generated to control the AGV to approach the reference plane, and the AGV approaching the reference plane result after the control command is executed is obtained; S4: The AGV approaching result after the control command is executed is evaluated, and the positioning control result is confirmed according to the evaluation result, and a positioning control result abnormal signal is obtained; S5: Feedback is given to the positioning control result abnormal signal, an early warning is issued according to the feedback abnormal signal, and the early warning signal is sent to the management terminal for human-machine interaction.

[0017] S1: Using the AGV's built-in positioning system, a global path is planned from the starting point to the target workstation located on the reference plane. The system determines in real-time whether the AGV has entered the precision positioning activation area and obtains the trigger signal for the AGV entering the precision positioning activation area. This includes the following steps: S1.1: First, the real-time global pose P1(X0,Y0,θ0) of the AGV is obtained through the AGV's built-in positioning system. X0 and Y0 are the two-dimensional coordinates of the AGV's starting position, and θ0 is the orientation angle of the AGV's starting position, indicating the AGV's orientation; then, based on the position P2(X0,Y0,θ0) of the target workstation where the reference plane is located... m ,Y m ,θ m ), X m and Y m Let θ be the two-dimensional coordinates of the target workstation location. m Using the reference plane orientation angle, a collision-free global path P from the starting point P1 to point P3 is planned using a global path planning algorithm. glo P glo=PathPlanning(P1,P3,Map), PathPlanning() is the path planning function, P3 is a target point in the activation area precisely located on the boundary of a circular area (e.g., a circular area of ​​1-3m) centered at P2 with a radius of R. The radius R is set according to the effective detection distance of the vision sensor (e.g., 2m) to ensure that the vision sensor can identify the marker after the AGV enters the area. Map is a global occupancy grid map of the environment; the global occupancy grid map is a two-dimensional matrix, and the value of each grid is in the range of [0,1]. A value of 1 indicates that the grid is definitely occupied by an obstacle (e.g., a wall, a fixed machine, etc.), a value of 0 indicates that the grid is definitely free and the AGV can pass through safely, a value of 0.5 indicates an unknown area that has not yet been explored or where the sensor information is contradictory, and a value between 0 and 1 indicates the possibility of being occupied. A value of 0.5 is a special dividing point, indicating "unknown" or "insufficient information". A value moving from 0.5 to 0 indicates that accumulated evidence supports that it is "free", and a value moving from 0.5 to 1 indicates that accumulated evidence supports that it is "occupied".

[0018] In this embodiment, it should be specifically noted that the positioning system built into the AVG can be a SLAM system (Simultaneous Localization) or a GPS / BeiDou system; all global positions and directions are represented in a two-dimensional global coordinate system. Within the AGV's driving area, a fixed reference point is set, the X-axis points along the horizontal ground in a fixed direction (such as the direction of the production line extension), the Y-axis is perpendicular to the X-axis, and the Z-axis is perpendicular to the ground and pointing upwards. The direction angle is based on the positive direction of the X-axis, with the counterclockwise direction being positive.

[0019] This embodiment specifically explains that the global path planning algorithm is an existing technology, such as the A* algorithm, Dijkstra's algorithm, or Quick Random Tree Traversal (RRT). Dijkstra's algorithm employs a greedy strategy, progressively expanding the known set of shortest paths until all reachable nodes are covered. The A* algorithm is an extension of Dijkstra's algorithm; when selecting which node to expand, it considers not only the actual cost from the starting point to the current node (as Dijkstra's algorithm does) but also the estimated cost from the current node to the destination. The RRT algorithm expands the tree structure incrementally, guaranteeing a feasible solution with probabilistic completeness in an infinite number of iterations. If a feasible connection sequence of length k exists, the expected number of iterations will not exceed k / p. When the tree size approaches infinity, the probability of the target point being included approaches 1. This algorithm allows for adaptation to different types of constraints by introducing control functions and supports a path backtracking mechanism to obtain the complete trajectory from the starting point to the destination.

[0020] S1.2: Based on global path P glo The AGV travels along the global path until it enters the precision positioning activation area, satisfying the requirements. R is the radius of the fine positioning activation area, (X,Y) is the real-time global position coordinates of the AGV, and the trigger signal for the AGV to enter the fine positioning activation area is obtained. Please see Figure 3 As shown, S2: Receive the trigger signal for the AGV to enter the precision positioning activation area, and obtain the real-time pose deviation of the AGV relative to the reference plane by fusing data from the vision sensor, lidar, and inertial measurement unit, including the following steps: S2.1: Based on the trigger signal of the AGV entering the precision positioning activation area, the pose (X) of the marker pre-set at the reference plane monitoring position in the camera coordinate system is first identified by a vision sensor (e.g., an industrial camera) installed on the AGV. c ,Y c ,θ c ), through the translation vector T of the VCS relative to the AGV in the vision sensor. c-v Z-axis rotation matrix R c-v and direction angle transformation function θ v Obtain the pose (X) of the marker in the AGV body coordinate system. v ,Y v ,θ v ), T c-v =(t x ,t y This refers to the installation offset of the camera relative to the center of the AGV vehicle (e.g., the camera is 0.5m in front of the center of the vehicle and 0.2m to the left). x =0.5, t y =0.2), γ is the rotation angle about the Z-axis, which is obtained through fixed parameters pre-calibrated by the camera relative to the AGV body coordinate system. θ v =θ c +γ; In this embodiment, it should be specifically noted that the camera coordinate system has the camera optical center as its origin, X... c The axis moves forward along the camera's optical axis, Y c Axis and X c The axis is perpendicular to the left, Z c The AGV body coordinate system is centered on the center of the AGV body, with the X axis pointing upwards. v The axis moves forward along the camera's optical axis, Y v Axis and X v Axis perpendicular, Z v Axially upward.

[0021] S2.2: Based on the pose of the marker in the AGV body coordinate system (X v ,Y v ,θ v The preset pose P of the preset marker in the local coordinate system of the reference plane.j P j =(X j ,Y j ,θ j Preset pose P j To fix the parameters, they are obtained through measurement. For example, if the marker is 0.8 m in front of and 0.1 m to the left of the center of the reference plane, and the orientation angle is consistent with the reference plane, then (X) j =0.8,Y j =0.1,θ j =0), through the position transformation function and the orientation angle transformation function θ b Obtain the pose P of the AGV in the local coordinate system of the reference plane. b P b =(X b ,Y b ,θ b The position transformation function is: R v-b Let R be the rotation matrix from the AGV body coordinate system to the local coordinate system of the reference plane. If the two initial directions are the same, then R v-b,s If it is an identity matrix, then construct a rotation matrix R based on the actual angle difference α. v-b,s , θ b =θ j -θ v ; This embodiment specifically explains how to convert the pose in the camera coordinate system to the pose in the AGV vehicle body coordinate system (VCS), and then further convert it to the pose in the local coordinate system of the reference plane (BCS). This needs to be achieved through a coordinate transformation matrix. The core is to utilize the sensor installation parameters (the position and attitude of the camera relative to the VCS) and the preset relationship between the reference plane and the VCS.

[0022] In this embodiment, it should be specifically noted that the precise positioning area is represented in a local coordinate system on a two-dimensional reference plane, with the center of the reference plane as the origin, X... j The axis is the docking direction along the reference plane toward the AGV, Y j The axis is perpendicular to X. j The axis lies in the reference plane and points to the left, Z j With the axis perpendicular to the reference plane and facing upwards, the poses of markers identified by the visual sensor and the poses of geometric features scanned by the lidar both need to be transformed into the local coordinate system of the reference plane.

[0023] S2.3: By scanning the reference plane of a pre-defined object using a lidar installed on the AGV body, the geometric features of the object (e.g., the geometric features of a vertical plate, the geometric features of a corner, etc.) are used to obtain the geometric feature point cloud set Q of the object, and then compared with the pre-defined standard geometric feature point cloud Q of the object. moICP point cloud matching is performed to obtain the pose P of the AGV relative to the preset object in the AGV body coordinate system. se P se =(X se ,Y se ,θ se Similarly, by presetting the pose P of the object... se Translation vector T relative to BCS v-b,j Rotation matrix R v-b,j and direction angle transformation function θ la Obtain the pose P of the AGV in the local coordinate system of the reference plane. la P la =(X la ,Y la ,θ la ), T v-b,j =(t x,j ,t y,j ), representing pose P se The position offset relative to the origin of the local coordinate system of the reference plane (for example, t is the position of the object 3m in front of and 0.5m to the left of the origin of the local coordinate system of the reference plane). x,j =3,t y,j =0.5), α1 is the pose P se The angle relative to the origin of the local coordinate system on the reference plane, θ la =θ se +α1; In this embodiment, it should be specifically noted that ICP point cloud matching is an existing technology. It calculates the optimal spatial transformation (rotation and translation) between two point clouds to minimize geometric errors, thereby obtaining the pose P of the lidar relative to a preset object. la It is widely used in fields such as 3D reconstruction, robot navigation, and autonomous driving.

[0024] S2.4: Measure the angular velocity w of the AGV using an inertial measurement unit. imu and acceleration a imu Integrating the acceleration yields the linear velocity, and integrating the linear velocity yields the AGV's two-dimensional position coordinates (X). imu,v ,Y imu,v Integrating the angular velocity yields the direction angle θ. imu,v Similarly, the translation vector T of the AGV relative to the BCS in the inertial measurement unit... v-b,I Rotation matrix R v-b,I and direction angle transformation function θ imu Obtain the pose P of the AGV in the local coordinate system of the reference plane. imu P imu =(X imu ,Y imu,θ imu ), T v-b,I =(t x,I ,t x,I The rotation matrix R is obtained by taking the angle α2 between the AGV and the origin of the local coordinate system relative to the reference plane. v-b,I , θ imu =θ imu,v -α2; In this embodiment, it is important to note that the IMU is a sensor that integrates a gyroscope and an accelerometer. The gyroscope directly measures the angular velocity of the AGV, and the accelerometer directly measures the acceleration. The "relative angle of the lidar relative to the VCS" is converted to the "absolute angle relative to the BCS". Therefore, the heading difference α1 between the VCS and the BCS needs to be superimposed. The absolute angle of the IMU relative to the VCS (i.e., the heading angle of the VCS itself) is converted to the "absolute angle relative to the BCS". Therefore, the heading difference α2 between the VCS and the BCS needs to be subtracted.

[0025] S2.5: Pose P in the local coordinate system of the reference plane obtained by fusing visual sensor, LiDAR and inertial measurement unit through extended Kalman filter (EKF). b P la and P imu First, define the state vector S, S=[X,Y,θ,V,W]. T X and Y are the two-dimensional position coordinates in the local coordinate system of the reference plane, θ is the AGV's driving direction angle, and V and W are the linear velocity and angular velocity of the AGV, respectively; then the state prediction equation S is defined. k S k =A×S k-1 +B×u k-1 +g k-1 Let k-1 represent the sampled data at time k-1, A be the state transition matrix, B be the control input matrix, and u be the input matrix. k-1 =[V k-1 W k-1 ] T g is the control variable for the AGV. k-1 The process noise follows a Gaussian distribution N(0,Q) k ), Q k The process covariance diagonal matrix is ​​composed of diagonal elements representing the variances of each element in the state vector, indicating that the noise of each state variable is independent, and has a dimension of 5×5; then, the observation equation Z is defined. k Z is used to describe the relationship between sensor observations and states. k =H×S k +m k Z k Z represents the sensor observation.k =[X b ,Y b ,θ b ,X la ,Y la ,θ la ,X imu ,Y imu ,θ imu ] T H is the observation matrix (mapping the state to the observation space), with dimensions of 9×5, mapping the pose components in the 5-dimensional state to the 9-dimensional observation vector. The velocity components are not mapped. k To observe the noise, we assume it follows a Gaussian distribution N(0,q). k ), q k To observe the covariance block diagonal matrix, each block diagonal matrix consists of a 3×3 matrix whose diagonal elements are the pose noise variance of each sensor. The noise covariance of each sensor can be determined through sensor calibration experiments or accuracy data provided by the manufacturer. The observation noise originates from the measurement errors of the vision sensor, lidar, and IMU. Secondly, the Kalman gain K... k By fusing the predicted and observed values, the final posterior state estimate S is obtained. k h S k h =S k +K k (Z k -H×S k Kalman gain K k It can be obtained through observation updates via extended Kalman filtering; finally, after each filtering cycle, the output posterior state estimate S is obtained. k h The first three elements are the real-time pose deviation ΔP of the AGV relative to the reference plane, where ΔP = [ΔX, ΔY, Δθ]. This embodiment needs to be specifically explained as follows: Δt is the sampling time, and k-1 represents the sampled data at time k-1. q b Let Variance be the pose noise variance matrix of the vision sensor. The attitude noise variance matrix logic of lidar and IMU is the same as that of vision sensors.

[0026] Please see Figure 4 As shown, S3: Based on the real-time pose deviation of the AGV relative to the reference plane, a local path is planned using the dynamic window method and motion control commands are generated to control the AGV to approach the reference plane, obtaining the AGV's approach to the reference plane result after the control commands are executed. This includes the following steps: S3.1: Based on the real-time pose deviation [ΔX, ΔY, Δθ] of the AGV relative to the reference plane, a feasible velocity set V is defined using the Dynamic Window Method (DWA). y V y ={(V,W)|V∈[V min V max ],W∈[W min W max ],V≤(2×dist(V,W)×a v,max ) 1 / 2 W≤(2×dist(V,W)×a) W,max ) 1 / 2}, V min and V max These are the minimum and maximum allowable linear speeds for the AGV, respectively, W. min and W max These are the minimum and maximum permissible angular velocities of the AGV, respectively, a v,max and a W,max These are the maximum linear acceleration and angular acceleration, respectively, and dist(V,W) is the distance to the nearest obstacle on the simulated trajectory; In this embodiment, it should be specifically noted that the dynamic window method is an existing technology. The DWA algorithm takes the current position and heading of the AGV as the starting point, uses the sampled velocity (V,W), and extrapolates (simulates) a short period of time (e.g., Δt = 1 to 3 seconds) based on the AGV's kinematic model. The result of the extrapolation is a simulated trajectory composed of multiple discrete pose points. For each pose point on this simulated trajectory, the algorithm performs the following operations: places the AGV's contour model (usually simplified to a circle or polygon) on this pose; calculates the minimum Euclidean distance between this contour and all the expanded obstacle meshes in the global cost map; after traversing all points on the trajectory, takes the minimum value among all these minimum distances as the dist(V,W) of this trajectory.

[0027] S3.2: The dynamic window method simulates a motion trajectory within a future time Δt based on each set of feasible velocities (V, W), and calculates the predicted pose deviation (ΔX) of the AGV at the trajectory endpoint. pre ,ΔY pre ,Δθ pre ), ΔX pre =ΔX-(V×Δt×cos(Δθ)), ΔY pre =ΔY-(V×Δt×sin(Δθ)), Δθ pre =Δθ-(W×Δt); S3.3: The dynamic window method calculates the score for each motion trajectory using the evaluation function G(V,W), where G(V,W) = a1 × he(Δθ). pre)+a2×dis(ΔX pre ,ΔY pre )+a3×ve(V)+a4×ob(V,W), he(Δθ pre ) represents the evaluation score for the direction angle, he(Δθ) pre )=1-(|Δθ pre | / π), Δθ is the direction angle deviation in radians, dis(ΔX) pre ,ΔY pre ) represents the predicted distance evaluation score of the AGV at the end of the trajectory, dis(ΔX) pre ,ΔY pre ) = 1 - (d / R), where d is the predicted distance of the AGV at the end of the trajectory, d = (ΔX 2 pre +ΔY 2 pre ) 1 / 2 ve(V) is the linear velocity evaluation score, R is the radius of the precise localization activation region, and ve(V) = V / V max ob(V,W) is the evaluation score of the distance dist(V,W) to the nearest obstacle on the simulated trajectory, and ob(V,W) = [dist(V,W) - d sa ] / d sa d sa For a safe distance, if ob(V,W)≤0, it is considered as 0. a1, a2, a3, and a4 are the corresponding weights, for example, a1=0.4, a2=0.3, a3=0.2, and a4=0.1. In scenarios with dense obstacles, increase a4 (obstacle avoidance weight) to 0.3; in open scenarios, increase a1 (heading weight) to 0.5. S3.4: The final generated motion control command is the velocity pair with the highest score in the evaluation function (V). best W best ), (V best W best =arg maxG(V,W), controls the AGV to approach the reference plane, and obtains the result of controlling the AGV to approach the reference plane, which is the pose deviation [ΔX] of the AGV relative to the reference plane after the control command is executed. aft ,ΔY aft ,Δθ aft ]; S4: Evaluate the AGV approximation result after the control command is executed, confirm the positioning control result based on the evaluation result, obtain the positioning control result abnormal signal, and determine the pose deviation [ΔX] of the AGV approximation result after the control command is executed. aft ,ΔY aft ,Δθ aft [Evaluate] if |ΔX aft |≤ex 、|ΔY aft |≤e y and |Δθ aft |≤e θ When all conditions are met, the positioning control is successfully confirmed via sensor ranging, and the current state of the AGV is locked. x e y and e θ These are the corresponding thresholds (e.g., e) x =5mm, e y =5mm and e θ =1°×π / 180); the sensor measures the AGV's pose deviation distance d. aft d=(ΔX 2 aft +ΔY 2 aft ) 1 / 2 If d aft ≤e d If d aft >e d If the position deviation is greater than the corresponding threshold, the positioning control result will be triggered as an abnormal signal. S5: Feedback is provided on abnormal positioning control results. An early warning is issued based on the feedback signal, and the warning signal is sent to the management terminal for human-machine interaction. Abnormal positioning control results are also fed back to S3 and S4 until the AGV's pose deviation relative to the reference plane is within the corresponding threshold range after the control command is executed. Conversely, if the pose deviation exceeds the set time T during the approach process in S3... th (For example, 5 seconds, determined based on the precision positioning convergence time experiment) If convergence cannot be achieved within the threshold, or sensor data is lost, positioning is considered a failure; the AGV will automatically execute a recovery procedure: first, the AGV retreats a safe distance R, then returns to S2; if the number of consecutive failures exceeds N... max If the error occurs 3 times (e.g., 3 times), the movement stops and feedback is provided. An early warning is issued based on the abnormal feedback signal, and the warning signal is sent to the management terminal for human-machine interaction. The time t for the pose deviation during the approach process in S3 is obtained from the difference between the current time and the time when the AGV enters S2, and t is greater than T. th or isInvalid(Z) k `isInvalid()` is a function to determine whether a sensor is valid (e.g., missing markers on a visual sensor, abnormal LiDAR data, etc.). k These are sensor observations.

[0028] Secondly: The accompanying drawings of the embodiments disclosed in this invention only involve the structures involved in the embodiments disclosed in this invention. Other structures can refer to the general design. In the absence of conflict, the same embodiment and different embodiments of this invention can be combined with each other. In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. An automated AGV datum plane positioning control method, characterized in that: include: S1: Through the AGV's built-in positioning system, a global path from the starting point to the target workstation located on the reference plane is planned, and the AGV is judged in real time whether it enters the fine positioning activation area, and the trigger signal for the AGV to enter the fine positioning activation area is obtained. S2: Receive the trigger signal for the AGV to enter the precision positioning activation area, and obtain the real-time pose deviation of the AGV relative to the reference plane by fusing data from the vision sensor, lidar and inertial measurement unit; S3: Based on the real-time pose deviation of the AGV relative to the reference plane, a local path is planned using the dynamic window method and motion control commands are generated to control the AGV to approach the reference plane, and the result of the AGV approaching the reference plane after the control commands are executed is obtained. S4: Evaluate the AGV approximation result after the control command is executed, confirm the positioning control result based on the evaluation result, and obtain the positioning control result abnormal signal; S5: Feedback is provided on abnormal signals of positioning control results, warnings are issued based on the feedback abnormal signals, and warning signals are sent to the management terminal for human-machine interaction.

2. The AGV reference plane positioning control method based on automation according to claim 1, characterized in that: The trigger signal in S1 for the AGV to enter the precision positioning activation area includes the following steps: S1.1: First, using the AGV's built-in positioning system, obtain the AGV's real-time global pose P1(X0,Y0,θ0), where X0 and Y0 are the two-dimensional coordinates of the AGV's starting position, and θ0 is the direction angle of the AGV's starting position; then, based on the position P2(X0,Y0,θ0) of the target workstation where the reference plane is located... m ,Y m ,θ m ), X m and Y m Let θ be the two-dimensional coordinates of the target workstation location. m Using the reference plane orientation angle, a collision-free global path P from the starting point P1 to point P3 is planned using a global path planning algorithm. glo P glo =PathPlanning(P1,P3,Map), PathPlanning() is the path planning function, P3 is a target point in the activated region on the boundary of a circular region with radius R centered at P2, and Map is the global occupied grid map of the environment. S1.2: Based on global path P glo The AGV travels along the global path until it enters the precision positioning activation area, satisfying the requirements. R is the radius of the precision positioning activation area, and (X,Y) are the real-time global position coordinates of the AGV. The trigger signal for the AGV to enter the precision positioning activation area is obtained.

3. The AGV reference plane positioning control method based on automation according to claim 1, characterized in that: The real-time pose deviation of the AGV relative to the reference plane in S2 is: the pose P in the local coordinate system of the reference plane obtained by fusing the vision sensor, lidar, and inertial measurement unit through extended Kalman filtering. b P la and P imu First, define the state vector S, S=[X,Y,θ,V,W]. T X and Y are the two-dimensional position coordinates in the local coordinate system of the reference plane, θ is the AGV's driving direction angle, and V and W are the linear velocity and angular velocity of the AGV, respectively; then the state prediction equation S is defined. k S k =A×S k-1 +B×u k-1 +g k-1 Let k-1 represent the sampled data at time k-1, A be the state transition matrix, B be the control input matrix, and u be the input matrix. k-1 =[V k-1 W k-1 ] T g is the control variable for the AGV. k-1 The process noise follows a Gaussian distribution N(0,Q) k ), Q k The process covariance diagonal matrix is ​​composed of diagonal elements representing the variances of each element in the state vector, indicating that the noise of each state variable is independent, and has a dimension of 5×5; then, the observation equation Z is defined. k Z k =H×S k +m k Z k Z represents the sensor observation. k =[X b ,Y b ,θ b ,X la ,Y la ,θ la ,X imu ,Y imu ,θ imu ] T H is the observation matrix with dimensions 9×5, mapping the pose components in the 5-dimensional state to the 9-dimensional observation vector. The velocity components are not mapped. k To observe the noise, we assume it follows a Gaussian distribution N(0,q). k ), q k To observe the covariance block diagonal matrix, each block diagonal matrix consists of a 3×3 matrix whose diagonal elements are the variance of the pose noise of each sensor; then, the Kalman gain K is used. k By fusing the predicted and observed values, the final posterior state estimate S is obtained. k h S k h =S k +K k (Z k -H×S k Finally, after each filtering cycle, the output posterior state estimate S k h The first three elements are the real-time pose deviation ΔP of the AGV relative to the reference plane, where ΔP = [ΔX, ΔY, Δθ].

4. The AGV reference plane positioning control method based on automation according to claim 1, characterized in that: The AGV approximation of the reference plane after the control command in S3 is executed includes: S3.1: Based on the real-time pose deviation [ΔX, ΔY, Δθ] of the AGV relative to the reference plane, a feasible velocity set V is defined using the dynamic window method. y V y ={(V,W)|V∈[V min V max ],W∈[W min W max ],V≤(2×dist(V,W)×a v,max ) 1 / 2 W≤(2×dist(V,W)×a) W,max ) 1 / 2 }, V min and V max These are the minimum and maximum allowable linear speeds for the AGV, respectively, W. min and W max These are the minimum and maximum permissible angular velocities of the AGV, respectively, a v,max and a W,max These are the maximum linear acceleration and angular acceleration, respectively, and dist(V,W) is the distance to the nearest obstacle on the simulated trajectory.

5. The AGV reference plane positioning control method based on automation according to claim 1, characterized in that: The AGV approximation of the reference plane after the control command in S3 is executed also includes: S3.2: The dynamic window method simulates a motion trajectory within a future time Δt based on each set of feasible velocities (V, W), and calculates the predicted pose deviation (ΔX) of the AGV at the trajectory endpoint. pre ,ΔY pre ,Δθ pre ); S3.3: The dynamic window method calculates the score for each motion trajectory using the evaluation function G(V,W); S3.4: The final generated motion control command is the velocity pair with the highest score in the evaluation function (V). best W best ), (V best W best =arg maxG(V,W), controls the AGV to approach the reference plane, and obtains the result of controlling the AGV to approach the reference plane, which is the pose deviation [ΔX] of the AGV relative to the reference plane after the control command is executed. aft ,ΔY aft ,Δθ aft ].

6. The AGV reference plane positioning control method based on automation according to claim 1, characterized in that: The abnormal positioning control result signal in S4 refers to the pose deviation [ΔX] of the AGV approximation result after the control command is executed. aft ,ΔY aft ,Δθ aft [Evaluate] if |ΔX aft |≤e x 、|ΔY aft |≤e y and |Δθ aft |≤e θ When all conditions are met, the positioning control is successfully confirmed via sensor ranging, and the current state of the AGV is locked. x e y and e θ These are the corresponding threshold values; the sensor measures the AGV's pose deviation distance d. aft d=(ΔX 2 aft +ΔY 2 aft ) 1 / 2 If d aft ≤e d If d aft >e d If the position deviation is greater than the corresponding threshold, the feedback will continue to S3 until the positioning is confirmed to be successful; otherwise, if any element of the position deviation is greater than the corresponding threshold, the positioning control result abnormal signal will be triggered.

7. The AGV reference plane positioning control method based on automation according to claim 1, characterized in that: The implementation of S5 includes: feeding back abnormal positioning control results to S3 and S4 until the pose deviation of the AGV relative to the reference plane is within the corresponding threshold range after the control command is executed; otherwise, if the pose deviation exceeds the set time T during the approximation process in S3, the AGV will not be able to move. th If the convergence fails to reach the threshold or sensor data is lost, the positioning is considered a failure; the AGV will automatically execute a recovery procedure: first, the AGV retreats a safe distance R, then returns to S2; if the number of consecutive failures exceeds N... max If the system stops moving and provides feedback, it will issue an early warning based on the abnormal feedback signal and send the warning signal to the management terminal for human-computer interaction.

Images