An AGV reference surface positioning control method based on automation

Abstract

The application discloses an AGV reference surface positioning control method based on automation, and particularly relates to the technical field of automatic guided vehicle control. First, whether the AGV enters the fine positioning activation area is judged through the positioning system built in the AGV. Then, the real-time pose deviation of the AGV relative to the reference surface is obtained by fusing the data of a visual sensor, a laser radar and an inertial measurement unit. Next, a local path is planned by using a dynamic window method and a motion control instruction is generated to control the AGV to approach the reference surface. The approaching result is evaluated, the positioning control result is confirmed according to the evaluation result, and the positioning control result abnormal signal is fed back to the management terminal for human-computer interaction. The application fuses multiple sensors by using the phased strategy of global coarse positioning and local fine positioning, effectively eliminates the limitations and cumulative errors of a single sensor, and further improves the positioning accuracy and accuracy of the AGV.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic guided vehicle control, in particular to an AGV reference surface positioning control method based on automation. BACKGROUND

[0002] An automatic guided vehicle (AGV) plays a vital role in modern intelligent warehousing and flexible manufacturing systems. One of its core performance indicators is the docking accuracy at target stations (such as charging stations, loading and unloading ports, and assembly tables). These target stations usually have a physical or virtual "reference surface", and the AGV needs to control the alignment error between its specific reference point and the reference surface to within millimeters.

[0003] In the prior art, the positioning control of AGV mainly relies on the following methods: dead reckoning, which estimates position and heading by measuring wheel speed; local positioning based on two-dimensional codes or reflective plates, which obtains absolute position by reading pre-set two-dimensional codes on the ground or scanning reflective plates at fixed positions; and SLAM positioning based on laser radars or vision, thereby achieving the purpose of automated AGV reference surface positioning control.

[0004] Current AGV reference surface positioning control methods have many shortcomings. For example, in terms of positioning accuracy, traditional magnetic strip navigation and two-dimensional code navigation are limited by the precision and installation error of navigation markers, making it difficult to meet high-precision positioning requirements. In terms of environmental adaptability, laser navigation AGVs are affected by the reflection of laser beams in strong light, dusty or uneven ground environments, leading to positioning deviation. Vision navigation AGVs are very sensitive to changes in lighting conditions, and the image information obtained by the camera may be distorted in unstable light environments, affecting the accuracy of positioning. In addition, when facing complex task scheduling and dynamic environmental changes, there is a lack of autonomous decision-making and real-time adjustment capabilities. In summary, the existing technology cannot meet the high-precision requirements and is prone to interference and poor stability in the final approach stage. Therefore, there is an urgent need in the art for an AGV reference surface positioning control method that can guarantee final positioning accuracy and has good flexibility and environmental adaptability. SUMMARY

[0005] To overcome the above-mentioned defects of the prior art, embodiments of the present application provide an AGV reference surface positioning control method based on automation to solve the problems raised in the background art.

[0006] To achieve the above-mentioned purpose, the present application provides the following technical solution: an AGV reference surface positioning control method based on automation, comprising:

[0007] S1: Plan the global path of the target station where the reference plane is located from the starting point to the target point through the positioning system built-in AGV, judge whether AGV enters the fine positioning activation area in real time, and obtain the trigger signal of AGV entering the fine positioning activation area;

[0008] S2: Receive the trigger signal of AGV entering the fine positioning activation area, obtain the real-time pose deviation of AGV relative to the reference plane by fusing the data of vision sensor, laser radar and inertial measurement unit;

[0009] S3: Based on the real-time pose deviation of AGV relative to the reference plane, the local path is planned and the motion control instruction is generated by using dynamic window method, the AGV is controlled to approach the reference plane, and the AGV approaching the reference plane result after executing the control instruction is obtained;

[0010] S4: The AGV approaching result after executing the control instruction is evaluated, the positioning control result is confirmed according to the evaluation result, and the positioning control result abnormal signal is obtained;

[0011] S5: Feedback the positioning control result abnormal signal, give an early warning according to the feedback abnormal signal, and send the warning signal to the management terminal for human-computer interaction.

[0012] The technical effects and advantages of the present application are:

[0013] 1, the present application is through the global coarse positioning + local fine positioning of the phased strategy, the multi-sensor of vision sensor, laser radar and IMU is fused by combining the extended Kalman filter algorithm, the limitation and cumulative error of single sensor are effectively eliminated, the stable and reliable pose feedback is provided in the final approaching stage, and then the AGV positioning accuracy and precision are improved;

[0014] 2, the dynamic window method is adopted in the local path planning to balance the heading alignment, safety obstacle avoidance, speed efficiency and other targets, so that the AGV can quickly converge to the pose deviation range in the fine positioning stage; at the same time, based on the final confirmation link of proximity sensor, double insurance is provided for positioning success, and the absolute reliability of docking with downstream equipment in the automation process is ensured;

[0015] 3, the present application feeds back the positioning control result abnormal signal, gives an early warning according to the feedback abnormal signal, has perfect fault detection and recovery mechanism, can independently try to recover when seriously disturbed or unexpected situation occurs, improves the autonomous decision and real-time adjustment ability of the whole system, and then helps to improve the precision of AGV reference plane positioning control. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 It is the overall flowchart of the present application.

[0017] Figure 2 A flowchart of the method of the present application.

[0018] Figure 3 A flowchart of the sensor fusion of the present application.

[0019] Figure 4 A flowchart of the AGV approaching the reference plane of the present application. DETAILED DESCRIPTION

[0020] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0021] Please refer to Figure 1 The present application provides an AGV reference plane positioning control system based on automation, which comprises 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.

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

[0023] The AGV reference plane initial positioning module: through the positioning system built-in the AGV, a global path from the starting point to the target point of the target station where the reference plane is located is planned, it is judged in real time whether the AGV enters the fine positioning activation area, and the trigger signal of the AGV entering the fine positioning activation area is transmitted to the multi-sensor fusion fine positioning module;

[0024] The multi-sensor fusion fine positioning module: receiving the trigger signal of the AGV entering the fine positioning activation area, through the fusion of the data of the vision sensor, the laser radar and the inertial measurement unit, the real-time pose deviation of the AGV relative to the reference plane is obtained and transmitted to the AGV reference plane positioning control module;

[0025] The AGV reference plane positioning control module: based on the real-time pose deviation of the AGV relative to the reference plane, a local path is planned and a motion control instruction is generated by using the dynamic window method, the AGV is controlled to approach the reference plane, and the approaching result of the AGV after executing the control instruction is transmitted to the AGV reference plane positioning control confirmation module.

[0026] AGV reference surface positioning control confirmation module: evaluate the AGV approaching result after the execution of the control instruction, confirm the positioning control result according to the evaluation result, and transmit the positioning control result abnormal signal to the AGV reference surface positioning control fault tolerance module;

[0027] AGV reference surface positioning control fault tolerance module: feedback the positioning control result abnormal signal, give a warning according to the feedback abnormal signal, and send the warning signal to the management terminal for human-computer interaction.

[0028] Please refer to Figure 2 The AGV reference surface positioning control method based on automation shown in the figure, comprising: S1: through the positioning system built-in AGV, planning a global path from the starting point to the target point as the target work station where the reference surface is located, judging whether the AGV enters the fine positioning activation area in real time, obtaining the trigger signal of the AGV entering the fine positioning activation area; S2: receiving the trigger signal of the AGV entering the fine positioning activation area, obtaining the real-time pose deviation of the AGV relative to the reference surface by fusing the data of the vision sensor, laser radar and inertial measurement unit; S3: based on the real-time pose deviation of the AGV relative to the reference surface, planning a local path and generating a motion control instruction by using dynamic window method, controlling the AGV to approach the reference surface, obtaining the AGV approaching reference surface result after the execution of the control instruction; S4: evaluating the AGV approaching result after the execution of the control instruction, confirming the positioning control result according to the evaluation result, obtaining the positioning control result abnormal signal; S5: feedback the positioning control result abnormal signal, give a warning according to the feedback abnormal signal, and send the warning signal to the management terminal for human-computer interaction.

[0029] S1: through the positioning system built-in AGV, planning a global path from the starting point to the target point as the target work station where the reference surface is located, judging whether the AGV enters the fine positioning activation area in real time, obtaining the trigger signal of the AGV entering the fine positioning activation area, comprising the following steps:

[0030] S1.1: first, through the positioning system built-in AGV, obtaining the real-time global pose P1(X0, Y0, θ0) of AGV, X0 and Y0 are the two-dimensional coordinates of the starting point position of AGV, θ0 is the direction angle of the starting position of AGV, indicating the orientation of AGV; then according to the position P2(X m ,Y m ,θ m ) of the target work station where the reference surface is located, X m and Y m are the two-dimensional coordinates of the target work station position, θ m is the direction angle of the reference surface, through the global path planning algorithm, planning a collision-free global path Pglo , P glo = PathPlanning(P1, P3, Map), PathPlanning() is a path planning function, P3 is a target point on the boundary of a circular region (for example, a 1-3 m circular region) centered at P2 with a radius R, the radius R is set according to the effective detection distance of the vision sensor (for example, 2 m), to ensure that the vision sensor can identify the marker after the AGV enters the region, and Map is a global occupancy grid map of the environment; the global occupancy grid map is a two-dimensional matrix, the value range of each grid is [0, 1], the value = 1 indicates that the grid is definitely occupied by an obstacle (for example, a wall, a fixed machine, etc.), the value = 0 indicates that the grid is definitely free and the AGV can pass safely, the value = 0.5 indicates an unknown region that has not been explored or the sensor information is contradictory, the value between 0 and 1 indicates the possibility of being occupied, and the value 0.5 is a special dividing point, indicating "unknown" or "insufficient information", the value moving from 0.5 to 0 indicates that the accumulated evidence supports it as "free", and the value moving from 0.5 to 1 indicates that the accumulated evidence supports it as "occupied".

[0031] It needs to be specifically explained in this embodiment that the positioning system built-in the AGV can be a SLAM system (instant positioning) or a GPS / Beidou system; all global positions and directions are represented in a two-dimensional global coordinate system, and a fixed reference point is set in the AGV driving area, the X axis points to a fixed direction (such as the extension direction of the production line) along the horizontal ground, the Y axis is perpendicular to the X axis, and the Z axis is perpendicular to the ground and upward, and the direction angle takes the positive direction of the X axis as the reference and the counterclockwise direction as positive.

[0032] It needs to be specifically explained in this embodiment that the global path planning algorithm is a prior art, for example, A* algorithm, Dijkstra algorithm or rapid-exploring random tree (RRT); the Dijkstra algorithm adopts a greedy strategy, gradually expands the known shortest path set until all reachable nodes are covered, the A* algorithm is an extension of the Dijkstra algorithm, and when selecting an expanded node, not only the actual cost from the starting point to the current node (as done by the Dijkstra algorithm) is considered, but also the estimated cost from the current node to the end point, the RRT algorithm expands a tree structure incrementally, and guarantees to find a feasible solution in an infinite number of iterations by probability completeness, and if there is a feasible connection sequence with a length of k, the expected number of iterations is not more than k / p; when the size of the tree tends to infinity, the probability of containing the target point tends to 1. The algorithm allows different types of constraints to be adapted by introducing a control function, and supports a path backtracking mechanism to obtain a complete trajectory from the starting point to the end point.

[0033] S1.2: based on the global path P glo , the AGV drives along the global path until it enters the fine positioning activation region, and satisfies , R is the radius of the fine positioning activation area, (X, Y) is the real-time global position coordinates of the AGV, and a trigger signal of the AGV entering the fine positioning activation area is obtained;

[0034] Please refer to Figure 3 S2: receiving the trigger signal of the AGV entering the fine positioning activation area, obtaining the real-time pose deviation of the AGV relative to the reference surface by fusing the data of the visual sensor, the laser radar and the inertial measurement unit, including the following steps:

[0035] S2.1: based on the trigger signal of the AGV entering the fine positioning activation area, first recognizing the pose (X c ,Y c ,θ c ) of the marker pre-set at the monitoring position of the reference surface in the camera coordinate system through the visual sensor (such as an industrial camera) installed on the AGV, obtaining the pose (X v ,Y v ,θ v ) of the marker in the AGV vehicle body coordinate system through the translation vector T c-v of the VCS in the visual sensor relative to the AGV, the Z-axis rotation matrix R c-v and the direction angle conversion function θ v , T c-v =(t x ,t y ), that is, the installation offset of the camera relative to the center of the AGV vehicle body (for example, the camera is 0.5m in front of the center of the vehicle body and 0.2m to the left, t x =0.5, t y =0.2), , γ is the rotation angle around the Z-axis, which is obtained through the fixed parameters of the pre-calibration of the camera relative to the AGV vehicle body coordinate system, , θ v =θ c +γ;

[0036] It needs to be specifically explained in this embodiment that the camera coordinate system takes the camera optical center as the origin, the X c axis is forward along the camera optical axis, the Y c axis is perpendicular to the X c axis and to the left, and the Z c axis is upward; the AGV vehicle body coordinate system takes the center of the AGV vehicle body as the origin, the X v axis is forward along the camera optical axis, the Y v axis is perpendicular to the X v axis, and the Z v axis is upward.

[0037] S2.2: based on the pose (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 ;

[0038] 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.

[0039] 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.

[0040] 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. mo ICP 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;

[0041] 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.

[0042] 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 conversion function θ imu get the pose P of the AGV in the reference plane local coordinate system imu , P imu = (X imu , Y imu , θ imu ), T v-b,I = (t x,I , t x,I ), represents the position offset of the AGV relative to the origin of the reference plane local coordinate system, and the rotation matrix R v-b,I is obtained through the angle α2 of the AGV relative to the origin of the reference plane local coordinate system imu , , θ imu,v = θ b - α2

[0043] The embodiment needs to be specifically explained that the IMU is a sensor integrated with 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 laser radar relative to the VCS is converted into the absolute angle relative to the BCS, so the heading difference α1 of the VCS and the BCS needs to be superimposed, and the absolute angle of the IMU relative to the VCS (that is, the heading angle of the VCS itself) is converted into the absolute angle relative to the BCS, so the heading difference α2 of the VCS and the BCS needs to be subtracted.

[0044] S2.5: Fuse the poses P la , P imu of the AGV in the reference plane local coordinate system obtained by the vision sensor, the laser radar and the inertial measurement unit through the extended Kalman filter (EKF) T , first define the state vector S, S = [X, Y, θ, V, W] k , X and Y are two-dimensional position coordinates in the reference plane local coordinate system, θ is the driving direction angle of the AGV, V and W are the linear speed and angular speed of the AGV driving respectively; then define the state prediction equation S k = A × S k-1 + B × u k-1 + g k-1 , k-1 represents the sampling data at the k-1 time, A is the state transition matrix, B is the control input matrix, u k-1 = [V k-1 , W k-1 ] T is the control amount of the AGV, g k-1 obeys the Gaussian distribution N(0, Q k ), Q kis a process covariance diagonal matrix, composed of diagonal element values of the variance of each element in the state vector, indicating that the noise of each state quantity is independent of each other, and the dimension is 5x5; then define the observation equation Z k , used to describe the relationship between sensor observation and state, Z k =HxS k +m k , Z k is the sensor observation value, Z k =[X b , Y b , θ b , X la , Y la , θ la , X imu , Y imu , θ imu ] T , H is an observation matrix (mapping the state to the observation space), with a dimension of 9x5, mapping the pose component in the 5-dimensional state to the 9-dimensional observation vector, and the velocity component is not mapped, m k is the observation noise, which follows the Gaussian distribution N(0, q k ), q k is an observation covariance block diagonal matrix, each block diagonal matrix is composed of diagonal element values of a matrix of each sensor pose noise variance, with a dimension of 3x3, and each sensor noise covariance can be determined through sensor calibration experiment or manufacturer's accuracy data. The observation noise is derived from the measurement error of the vision sensor, laser radar and IMU; secondly, the predicted value and the observation value are fused through the Kalman gain K k to obtain the final posterior state estimation S k h , S k h =S k +K k (Z k -HxS k ), the Kalman gain K k can be obtained by extending the Kalman filter observation update; finally, after each filtering cycle, the first three elements of the output posterior state estimation S k h are the real-time pose deviation ΔP of the AGV relative to the reference plane, ΔP=[ΔX, ΔY, Δθ];

[0045] The embodiment needs to be specifically explained , Δt is the sampling time, k-1 represents the sampling data at the k-1 time, , q b is the pose noise variance matrix of the vision sensor, the pose noise variance matrix logic of the laser radar and the IMU is the same as that of the vision sensor.

[0046] 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:

[0047] 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, a and a', respectively. 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;

[0048] 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.

[0049] 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 ), ΔXpre = ΔX - (V x Δt x cos(Δθ)), ΔY pre = ΔY - (V x Δt x sin(Δθ)), Δθ pre = Δθ - (W x Δt);

[0050] S3.3: Dynamic window method calculates the score of each motion trajectory by evaluation function G(V, W), G(V, W) = a1 x he(Δθ pre ) + a2 x dis(ΔX pre , ΔY pre ) + a3 x ve(V) + a4 x ob(V, W), he(Δθ pre ) is the heading angle evaluation score, he(Δθ pre ) = 1 - (|Δθ pre | / π), Δθ is the heading angle deviation in radians, dis(ΔX pre , ΔY pre ) is the predicted distance evaluation score of AGV at the end of the trajectory, dis(ΔX pre , ΔY pre ) = 1 - (d / R), d is the predicted distance of AGV at the end of the trajectory, d = (ΔX 2 pre + ΔY 2 pre ) 1 / 2 , ve(V) is the linear speed evaluation score, R is the radius of the fine positioning activation area, ve(V) = V / V max , ob(V, W) is the evaluation score of the distance dist(V, W) from the nearest obstacle on the simulated trajectory, ob(V, W) = [dist(V, W) - d sa ] / d sa , d sa is the safety distance, ob(V, W) is considered as 0 if ob(V, W) ≤ 0, a1, a2, a3 and a4 are the respective weights, for example, a1 = 0.4, a2 = 0.3, a3 = 0.2 and a4 = 0.1, in the obstacle-dense scene, increase a4 (obstacle avoidance weight) to 0.3; in the open scene, increase a1 (heading weight) to 0.5;

[0051] S3.4: The final motion control command is the speed pair (V best , W best ) with the highest evaluation function score, (V best , W best ) = arg max G(V, W), control the AGV to approach the reference surface, and the result of controlling the AGV to approach the reference surface is the pose deviation [ΔX aft , ΔYaft , Δθ aft ];

[0052] S4: evaluating the approaching result of the AGV after the execution of the control instruction, confirming the positioning control result according to the evaluation result, obtaining the positioning control result abnormal signal, evaluating the pose deviation [ΔX aft , ΔY aft , Δθ aft ] of the approaching result of the AGV after the execution of the control instruction, if |ΔX aft |≤e x , |ΔY aft |≤e y and |Δθ aft |≤e θ are all satisfied, confirming the positioning control success result through sensor ranging, locking the current state of the AGV, e x , e y and e θ are the corresponding threshold values (for example, e x =5mm, e y =5mm and e θ =1°×π / 180); the sensor ranging measures the pose deviation distance d aft of the AGV, d=(ΔX 2 aft +ΔY 2 aft ) 1 / 2 , if d aft ≤e d , the positioning is confirmed to be successful; if d aft >e d , the feedback is continued to S3 until the positioning is confirmed to be successful; otherwise, if any element in the pose deviation is greater than the corresponding threshold value, the positioning control result abnormal signal is triggered;

[0053] S5: feeding back the positioning control result abnormal signal, giving a warning according to the feedback abnormal signal, sending the warning signal to the management terminal for human-computer interaction, feeding back the positioning control result abnormal signal to S3 and S4 until the pose deviation of the AGV relative to the reference plane after the execution of the control instruction is within the corresponding threshold value range; otherwise, if the pose deviation exceeds the threshold value within the set time T th (for example, 5s, determined according to the precision positioning convergence time experiment) or the sensor data is lost, the positioning is determined to be failed; the AGV will automatically execute a recovery program: first, the AGV retreats a safe distance R, and then returns to S2; if the number of continuous failures exceeds N maxIf the number of times of the deviation is less than or equal to a preset threshold (for example, 3 times), the movement is stopped to give feedback, a warning is given according to the feedback exception signal, and the warning signal is sent to a management terminal to give human-computer interaction; a pose deviation time t in the approaching process of S3 is obtained by a difference between a current time and a time when the AGV enters S2, and t is greater than T th or isInvalid(Z k ), isInvalid() being a function for judging whether a sensor is valid (for example, a visual sensor marker is lost, laser radar data is abnormal, etc.), and Z k being a sensor observation value.

[0054] Secondly, in the drawings of the disclosed embodiments, only structures related to the disclosed embodiments are involved, other structures can be referred to common designs, and in the case of no conflict, the same embodiments and different embodiments of the present application can be combined with each other;

[0055] Finally, the above only describes preferred embodiments of the present application, and is not used to limit the present application, and any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

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, obtaining the AGV's approach to the reference plane result after the control commands are executed. The AGV's approach to the reference plane result 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, a and a', respectively. 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; 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 ]; 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 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.

5. 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