Navigation method for high-speed unloading robot for cartons of van

Abstract

The invention discloses a high-speed unloading robot navigation method for van cartons, relates to the technical field of robot navigation, and aims to solve the problem that an existing navigation algorithm depends on a two-dimensional grid map and cannot be finely adjusted. Therefore, the problem of low navigation precision is solved. According to the invention, the radar sensor is adopted to collect environmental point cloud data, and external parameter calibration is carried out on each radar to realize spatial registration and fusion of multi-source point clouds. After the high-quality fusion point cloud is obtained, plane detection and navigation path extraction are further carried out, and finally fine adjustment is carried out in combination with pose data. And the navigation precision is improved.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a navigation method for a high-speed unloading robot for cartons from a van. Background Art

[0002] The van carton unloading robot is an automated logistics device primarily used to automatically unload cartons from trucks, containers, or vans. It is suitable for warehousing logistics, e-commerce sorting, and manufacturing handling. It can replace manual handling, improving efficiency, reducing labor intensity, and minimizing the risk of cargo damage.

[0003] Navigation technology is a core function of a van-unloading robot, directly impacting operational efficiency and reliability. Existing ROS navigation algorithms rely on two-dimensional grid maps and lack fine-tuning capabilities, resulting in low navigation accuracy. Summary of the Invention

[0004] The purpose of the present invention is to provide a navigation method for a high-speed unloading robot for cartons, in order to address the problem that existing navigation algorithms rely on two-dimensional grid maps and cannot be fine-tuned, resulting in low navigation accuracy.

[0005] The technical solution adopted by the present invention to solve the above technical problems is:

[0006] A navigation method for a high-speed unloading robot for cartons in a van includes the following steps:

[0007] For each carton working surface, perform the following steps:

[0008] Step 1: Use multi-radar point cloud stitching and frame generation technology to obtain a fused point cloud frame P fuse (t);

[0009] Step 2: Point cloud P fuse (t) Perform filtering to obtain the point cloud P;

[0010] Step 3: Based on the point cloud P, and using the RANSAC algorithm, determine the angle difference Δθ between the ideal orientation of the robot and the current orientation of the robot, as well as the distance difference v between the ideal position of the robot and the current position of the robot in the x and y directions. x 、v y ;

[0011] Step 4: Use Δθ, v x and v y As input, and through rigid body motion translation, determine the linear velocity v of the robot's driving wheel in the robot coordinate system i and the position P of the driving wheel in the robot coordinate system i ;

[0012] Step 5: Repeat steps 1 to 4 to achieve robot navigation.

[0013] Furthermore, the step 1 is specifically as follows:

[0014] Step 1: Obtain the pose transformation matrix of the radar relative to the robot coordinate system through external parameter calibration;

[0015] Step 1 and 2: Use the pose transformation matrix to construct the rigid body transformation T of the radar sensor i ;

[0016] Step 13: Use the radar sensor to collect the original point cloud and convert it into a homogeneous vector Then use the homogeneous vector Combined with rigid body transformation T i , get the coordinates of the point cloud in the robot coordinate system;

[0017] Step 14: Set the refresh cycle of the radar sensor, and perform rigid body transformation registration on the point cloud collected within the refresh cycle, and then fuse them to obtain the point cloud P fuse (t).

[0018] Furthermore, the step 2 is specifically as follows:

[0019] Step 21: Use voxel grid method to quantize point cloud P fuse Perform downsampling;

[0020] Step 22: Use the x, y, and z axes in the robot coordinate system and the set boundary threshold to perform the down-sampled point cloud P. fuse Perform cropping and retain the point cloud within the cropped range;

[0021] Step 2 and 3: Construct a Kd-Tree model based on the retained point cloud, and obtain the k point clouds closest to each point cloud based on the Kd-Tree model. Then, based on the Euclidean distance between this point cloud and each point cloud in the k point clouds, obtain the average Euclidean distance d corresponding to the point cloud. i ;

[0022] Step 24: According to the average Euclidean distance d corresponding to each point cloud i , take the average value, and get the average value

[0023] Step 25: According to the average Euclidean distance d i and average Calculate the standard deviation σ, and remove outliers based on the standard deviation σ and the set threshold to obtain the point cloud P.

[0024] Furthermore, in step 21, the voxel grid method is used to map the point cloud Pfuse Downsampling is expressed as:

[0025] V(u,v,w)=[ul,(u+1)l)×[vl,(v+1)l)×[wl,(w+1)l)

[0026]

[0027] Among them, V(u,v,w) is the voxel index obtained after dividing the three-dimensional space, u,v,w are the integer grid numbers of the coordinate axis directions of x,y,z, respectively, c uvw is the centroid of all points falling into the voxel V(u,v,w), q is the coordinate vector of an original point cloud point within the voxel V(u,v,w), and l is the length of the voxel side.

[0028] Furthermore, the step three is specifically as follows:

[0029] For the same carton working surface, first determine the Δθ and v obtained in the previous frame y , whether it satisfies v y Less than 0.01, and Δθ less than 1°. If not satisfied, the robot is adjusted to align with the carriage opening. Otherwise, the robot is adjusted to follow the center line of the carriage inner wall. If the current frame is the initial frame, the robot is adjusted to align with the carriage opening.

[0030] The steps to adjust the robot's carriage opening alignment are:

[0031] Step A1: Perform a straight-through filter on the point cloud P in the x-direction to obtain the point cloud C, which is expressed as:

[0032]

[0033] in, The maximum and minimum values ​​in the x-direction preset for carriage opening alignment;

[0034] Step A2: Use the RANSAC algorithm to extract the target plane model in point cloud C. RANSAC randomly selects three points each time to construct a spatial plane and calculates (n, d), where n is the unit normal vector of the current plane and d is the offset from the current plane to the origin. Based on the constraint conditions, traverse the entire point cloud to obtain the target plane, that is, the plane of the car door when the cartons are fully loaded. The constraint conditions are expressed as:

[0035]

[0036] inlier(n,d)={p∈C||n·p+d∣≤δ},|inlier(n,d)| <points

[0037] Among them, δ is the maximum distance from a point in the plane to the fitting plane, points is the minimum number of point clouds in the fitting plane, and ε is the angle between the normal vector of the fitting plane and the positive half axis of the x-axis of the vehicle coordinate system;

[0038] Step A3: Normalize the normal vector of the target plane and find the closest point c0 to the origin on the plane. c0 is expressed as:

[0039] c0=-dn

[0040]

[0041] Where a, b, c, and d are the coefficients of the plane equation formula, ax+by+cz+d=0;

[0042] Step A4: Extract the points in the target plane, obtain the extreme values ​​of the point cloud coordinates in the y and z directions, and define the ROI boundary to surround the extreme value range. Then obtain the center point c of the ROI area and project the ROI normal vector on the XY plane to obtain the vector d xy , expressed as:

[0043]

[0044]

[0045] Among them, c 0,x Indicates the x-coordinate value of point c0, n x ,n y Represent the components of the unit normal vector n in the x and y directions respectively;

[0046] Step A5: Based on the result of step A4, determine the angle difference Δθ between the ideal orientation of the robot and the current orientation of the robot, and the distance difference v between the ideal position of the robot and the current position of the robot in the x and y directions. x 、v y , expressed as:

[0047]

[0048] Among them, θ IMU is the current orientation of the robot, c x ,c y are the coordinate values ​​of the center point c in the x and y directions respectively, and τ is the threshold of the distance from the robot coordinate origin to the carriage entrance in the x direction;

[0049] The steps for adjusting the robot to follow the center line of the inner wall of the carriage are as follows:

[0050] Step B1: Perform straight-through filtering on the point cloud P in the x and z directions to obtain the point cloud S, which is expressed as:

[0051]

[0052] in, They are the maximum and minimum values ​​in the x and z directions when walking along the center line of the inner wall of the carriage.

[0053] Step B2: Divide the point cloud S into the left point cloud S according to the coordinates in the y direction l With the right point cloud S r Then, the RANSAC algorithm is used to extract the fitting planes n on the left and right sides of the carriage wall. L and n R , and calculate the unit normal vector, n L and n R Expressed as:

[0054]

[0055]

[0056] Among them, a L ,b L ,c L ,d L ,a R ,b R ,c R ,d R is the coefficient of the plane equation formula, a L x+b L y+c L z+d L =0,a R x+b R y+c R z+d R =0;

[0057] Step B3: Project the unit normal vector onto the XY plane to obtain the horizontal truncation and Among them, n L,x ,n L,y ,n R,x ,n R,y n L ,n R Components in the x and y directions;

[0058] Step B4: Cut off horizontally and Get the midline tangent vector d raw , expressed as:

[0059] d raw (d x ,d y ,0)=n l ×nr ;

[0060] Among them, d x d y are the midline tangent vectors d raw Components in the x and y directions;

[0061] Step B5: According to the midline tangent vector d raw , get the midline tangent unit vector d corr , expressed as:

[0062]

[0063] Step B6: According to n L 、n R d L and d R , get the closest point p to the origin on the left and right planes L 、p R and their geometric midpoint positions p mid , expressed as:

[0064] p L =-d L n L

[0065] p R =-d R n R

[0066]

[0067] Step B7: Based on d corr and p mid , introduce the smoothing coefficient α, and get the tangent unit vector of the current frame and the geometric midpoint position of the left and right planes of the current frame Expressed as:

[0068]

[0069] in, and is the tangent unit vector of the midline of the previous frame and the geometric midpoint position of the left and right planes;

[0070] Step B8: According to and Determine the angle difference Δθ between the ideal robot orientation and the current robot orientation, and the distance difference v between the ideal robot position and the current robot position in the x and y directions. x 、v y , expressed as:

[0071]

[0072] in, are vectors The components in the x and y directions, ∈, are the preset robot forward speeds. for The coordinate value in the y direction.

[0073] Furthermore, the linear velocity v of the driving wheel of the robot in the robot coordinate system is i Expressed as:

[0074]

[0075] Among them, x i 、y i are the coordinates of the current driving wheel in the x and y directions, respectively, and ω is the angular velocity of rotation regulated according to Δθ.

[0076] Furthermore, the position P of the driving wheel in the robot coordinate system i Expressed as:

[0077]

[0078] Furthermore, the robot is provided with four wheels, which are symmetrically distributed. The left front wheel and the right rear wheel are driving wheels. The steering wheel angle and linear velocity of the left front wheel and the right rear wheel are expressed as:

[0079]

[0080] Among them, W and L are the lengths of the driving wheel symmetrical distribution rectangle in the x and y directions respectively, θ F ,θ R are the absolute encoding angles of the left and right driving wheels in the current frame, s F ,s R They are respectively the linear velocities of the left and right driving wheels in the current frame.

[0081] Furthermore, the rigid body transformation T i Expressed as:

[0082]

[0083] Among them, R i is the rotation amount, R i =R z (ψ i )R y (θ i )R x (φ i ), R z (ψ i ), R y (θi ), R x (φ i ) are the rotation amounts relative to the z, y, and x axes, respectively, and t i is the translation, SE(3) represents the 3D special Euclidean group.

[0084] Furthermore, the refresh period of the radar sensor is 200ms.

[0085] The beneficial effects of the present invention are:

[0086] This application uses radar sensors to collect environmental point cloud data and calibrates the external parameters of each radar to achieve spatial registration and fusion of multi-source point clouds. After obtaining a high-quality fused point cloud, plane detection and navigation path extraction are further performed, and finally, fine-tuning is performed based on the pose data, thereby improving navigation accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 A visual diagram for navigating this application. DETAILED DESCRIPTION

[0088] It should be noted that, unless there is any conflict, the various embodiments disclosed in this application can be combined with each other.

[0089] Specific embodiment 1: The navigation method of a high-speed unloading robot for cartons in this embodiment includes the following steps:

[0090] For each carton working surface, perform the following steps:

[0091] Step 1: Use multi-radar point cloud stitching and frame generation technology to obtain a fused point cloud frame P fuse (t);

[0092] Step 2: Point cloud P fuse (t) Perform filtering to obtain the point cloud P;

[0093] Step 3: Based on the point cloud P, and using the RANSAC algorithm, determine the angle difference Δθ between the ideal orientation of the robot and the current orientation of the robot, as well as the distance difference v between the ideal position of the robot and the current position of the robot in the x and y directions. x 、v y ;

[0094] Step 4: Use Δθ, v x and v y As input, and through rigid body motion translation, determine the linear velocity v of the robot's active wheel in the robot coordinate system i and the position P of the driving wheel in the robot coordinate system i ;

[0095] Step 5: Repeat steps 1 to 4 to achieve robot navigation.

[0096] Step 1. Multi-radar point cloud stitching and frame generation

[0097] The vehicle body is equipped with four radar sensors, which are distributed around the vehicle to obtain all-round point cloud data. The pose transformation matrix (rollφ, pitchθ, yawψ, t x,i ,t y,i ,t z,i ), use the pose transformation matrix to construct the rigid body transformation T corresponding to the radar i , expressed as:

[0098]

[0099] Among them, R i =R z (ψ i )R y (θ i )R x (φ i ), calculated from the Euler angles (rollφ, pitchθ, yawψ, unit: °), is the translation amount (unit: m), R i is the rotation amount, R z (ψ i )R y (θ i )R x (φ i ) are the rotation amounts relative to the z, y, and x axes, respectively, and SE(3) represents the 3D special Euclidean group.

[0100] In the coordinate system of the i-th radar, the original point cloud collected by LiDAR[i] at time t is recorded as:

[0101]

[0102] Among them, N i is the total number of point clouds, is the point cloud coordinate;

[0103] The single point cloud under each radar coordinate system Convert to a homogeneous vector The coordinates of the point cloud in the vehicle coordinate system are expressed as:

[0104]

[0105] in, is the homogeneous vector containing the point cloud coordinates in the vehicle coordinate system, is the point cloud in the vehicle coordinate system,

[0106] Since the platform is developed under Windows system and is limited by MID360 SDK, the data stream point cloud returned by the radar is not presented in the form of frames, but is called in the form of 96 point clouds per group under non-repeated scanning. Therefore, this application defines the radar refresh period Δt = 200ms, and performs rigid body transformation registration of the corresponding sampling points on the point set collected by LiDAR[i] (i = 0, 1, 2, 3) within the same integration window Δt, and fuses them to form a frame of point cloud P fuse (t), is expressed as:

[0107]

[0108] Among them, ∪ is a set;

[0109] At the end of each cycle, the main thread clears the cache and reserves capacity to prepare for re-accumulation in the next cycle. Δt = 200ms is defined to generate fused point cloud frames at 5Hz.

[0110] Step 2. Fusion point cloud preprocessing

[0111] Get the environmental information point cloud P in the current frame vehicle coordinate system fuse In order to improve the subsequent calculation efficiency and filter out noise, the fused point cloud needs to be preprocessed in multiple stages, mainly including:

[0112] Voxel Grid downsampling: The volume of the initial point cloud data single frame of 300,000 point clouds brings a huge computational cost, so the voxel grid method is used to filter the point cloud P fuse Downsampling is performed. Given a voxel side length l, the point cloud space is divided into a 3D voxel grid, retaining only the centroid within each voxel grid, reducing the number of points while retaining the basic structural features of the environment. It is expressed as:

[0113] V(u,v,w)=[ul,(u+1)l)×[vl,(v+1)l)×[wl,(w+1)l) (5)

[0114]

[0115] Among them, V(u,v,w) represents the voxel index obtained after dividing the three-dimensional space, and u,v,w correspond to the integer grid numbers in the directions of the three coordinate axes x, y, and z respectively. uvw is the centroid of all points falling into the voxel V(u,v,w), and q is the coordinate vector of an original point cloud point located in the voxel V(u,v,w);

[0116] Through filtering: The downsampled point cloud is clipped along the three coordinate axes x, y, and z (vehicle coordinate system) to remove points outside the range. Specifically, the boundary threshold X of the region of interest is set in the vehicle coordinate system. min ,X max ,Y min ,Y max ,Z min ,Z max Only points that fall within the above range are retained to filter out irrelevant point clouds that are too far away or too high / too low. It is expressed as:

[0117]

[0118] Statistical filtering: After the above processing, in order to further remove the residual background information and discrete noise information in the point cloud data, the statistical filter is used for optimization. A Kd-Tree model of point cloud data is constructed, and for each point p i (x i ,y i ,z i ) Find the k points closest to it {p j ;j=1.2.3..k};p j (x j ,y j ,z j ), calculate p i The average Euclidean distance d between the neighboring points i :

[0119]

[0120] Traverse all points in the point cloud and calculate d i Average value And calculate the standard deviation σ based on them, based on the calculation results and the preset threshold T t1 Filter the point cloud data. Remove outliers, where Adopt the trust region idea to ensure that the main data is preserved. and σ can be calculated:

[0121]

[0122] In general, the preprocessing process can be expressed as follows:

[0123] P=F v (F z (F t (P fuse ))) (10)

[0124] in represents the preprocessed point cloud, F v (·), F z (·), F t (·) represent the voxel filtering, pass filtering and statistical filtering processes respectively.

[0125] Step 3. Navigation function selection

[0126] Based on the usage requirements of the device, this application has designed two navigation functions:

[0127] 1. Detect the carriage opening and realize automatic alignment.

[0128] 2. Walk in a straight line inside the carriage.

[0129] On each carton working surface, the carriage mouth alignment is first performed, and then according to the judgment condition, that is, when v y When it is less than 0.01 and Δθ is less than 1°, the centerline adjustment of the inner wall of the carriage will be performed in the next frame and all subsequent frames.

[0130] Carriage opening alignment

[0131] Based on the fact that the key features such as the current car door are located on the front side of the car body, the input point cloud P is first subjected to secondary straight-through filtering in the x direction to focus on the key feature area, which is expressed as:

[0132]

[0133] The maximum and minimum values ​​in the x-direction are preset for alignment of the carriage opening.

[0134] Next, use the optimized RANSAC algorithm to extract the target plane model in the current point cloud C, adding the following constraints:

[0135] 1. The maximum distance δ from a point in the plane to the fitting plane.

[0136] 2. The minimum number of point clouds in the fitting plane.

[0137] 3. The angle between the normal vector of the fitting plane and the positive half axis of the x-axis of the vehicle coordinate system is not greater than ε.

[0138] RANSAC randomly selects three points each time to construct a spatial plane and calculates (n, d), where n is the unit normal vector of the current plane and d is the offset from the current plane to the origin. Based on the above constraints, the target plane is obtained by traversing the entire point cloud, which is the plane of the car door when the cartons are fully loaded. The conditions are expressed as:

[0139]

[0140] Here, inlier(·,·) represents the current plane, and |inlier(·,·)| represents the number of points in the current plane.

[0141] Normalize the normal vector of the fitted target plane ax+by+cz+d=0 and find the closest point c0 on the plane to the origin, which is expressed as:

[0142]

[0143] Where a, b, c, and d represent the coefficients of the plane equation formula.

[0144] Extract the point set in the plane and identify the extreme values ​​of the point cloud coordinates in the y and z directions And define the ROI boundary to surround it (extreme value) to determine the size of the compartment opening plane. According to the extreme value size, calculate the center point of the ROI area and calculate the projection of the ROI normal vector on the XY plane to obtain the vector d xy ;

[0145] The center point c of the ROI area is expressed as:

[0146]

[0147] Among them, c 0,x Indicates the x-coordinate value of point c0, n x ,n y Represent the components of the unit normal vector n in the x and y directions respectively.

[0148] Therefore, based on the direction given by this vector, a unified interface for subsequent navigation instructions can be defined, including the vector and rotation angle of the vehicle coordinate system origin in the current frame. The angle is defined as positive in counterclockwise direction and negative in clockwise direction. The same applies to the driving wheel in the following text. The unified interface is expressed as:

[0149]

[0150] Among them, θ IMU Represents the current orientation of the vehicle. Since the reference coordinate system is the vehicle coordinate system, the positive half axis of the x-axis is the orientation of the vehicle, so here θ IMU = 0. is the angle difference between the ideal vehicle orientation and the current vehicle orientation, v x ,v y are defined interface parameters, representing the distance difference between the ideal position of the vehicle and the current position of the vehicle in the x and y directions, respectively. x ,c y They represent the coordinate values ​​of the center point c in the x and y directions respectively, and τ represents the manually preset threshold value from the center of the vehicle body to the car door in the x direction.

[0151] Run along the center line of the inner wall of the carriage

[0152] Similar to the previous method, based on the fact that key features such as the side walls of the current car are located on the front and rear sides of the car body, and taking into account ground interference factors, the input point cloud p is first subjected to secondary straight-through filtering in the x and z directions to focus on the key feature areas, which is expressed as:

[0153]

[0154] in, They are respectively the maximum and minimum values ​​in the x and z directions when walking along the center line of the inner wall of the carriage.

[0155] Here, this application divides the point cloud S into the left point cloud S according to the coordinate symbol of the y direction l With the right point cloud S r Then, the optimized RANSAC algorithm is used to extract the fitting planes of the inner walls of the left and right sides of the car respectively. Unlike the above limit on the angle between the normal vector of the fitting plane and the positive half axis of the x-axis, the object defined here becomes the positive half axis of the y-axis. Based on this, the fitting planes of the inner walls of the car on both sides can be obtained: a L x+b L y+c L z+d L =0,a R x+b R y+c R z+d R = 0, and calculate the unit normal vector:

[0156]

[0157] Among them, a L ,b L ,c L ,d L ,a R ,b R ,c R ,d R Represents the coefficients of the plane equation formula.

[0158] Project the unit normal vector onto the XY plane to obtain the horizontal truncation where n L,x ,n L,y ,n R,x ,n R,y Represents n L ,n R Components in the x and y directions. Further calculate the depth direction of the corridor, that is, the midline tangent vector d raw :

[0159] d raw (dx ,d y ,0)=n l ×n r (18)

[0160] Force normalization and delete its z component to obtain the midline tangent unit vector d corr :

[0161]

[0162] This application sets the driving mode category to forward and reverse at the outer end of the center line function. If the current driving mode is forward, it is required to x >0, if the current driving mode is reverse, d x <0.

[0163] Then calculate the closest point p to the origin on the left and right planes L ,p R and their geometric midpoint p mid :

[0164]

[0165] In each frame of the point cloud, the corridor centerline direction vector d corr and anchor point p mid All are obtained by fitting the point cloud of the current frame, which is affected by LiDAR noise, RANSAC randomness and occlusion of small obstacles. corr and p mid There will be flickering between frames, and the direction and position control instructions will undergo continuous mutations. corr and p mid Perform a first-order low-pass time filter, refer to the historical frame data to retain the trend, suppress high-frequency noise, and smooth the control command. Set the smoothing coefficient α, the previous frame parameter The current frame parameters The calculation process can be expressed as:

[0166]

[0167] Similar to formula (15), according to The unified interface for calculating the navigation instructions outputted by the current frame Δθ,v x ,v y :

[0168]

[0169] in, Represents vectors The components in the x and y directions, ∈, represent the artificially preset vehicle forward speed. Representative Points The coordinate value in the y direction.

[0170] Step 4. Driving wheel motion calculation

[0171] Unified interface Δθ,v with navigation instructions x ,v y As input, the origin of the vehicle coordinate system is the center, and the four wheels of the vehicle are symmetrically distributed, of which the left front wheel and the right rear wheel are driving wheels and are diagonally distributed. For any driving wheel, through rigid body motion translation, its linear velocity in the vehicle coordinate system satisfies:

[0172]

[0173] Among them, P i Represents the current position of the driving wheel in the vehicle coordinate system, x i ,y i are the x and y coordinates respectively. ω is the manually controlled angular velocity, which can be adjusted according to Δθ.

[0174] By analyzing the superposition of the rotational and translational motion states, the final steering wheel angles and linear velocities of the two driving wheels are calculated respectively:

[0175]

[0176] Where W and L represent the distances of the driving wheel symmetrically distributed rectangular positions in the x and y directions respectively. F ,θ R Represents the absolute encoding angle of the left and right driving wheels in the current frame, s F ,s R Respectively represent the linear speed of the left and right driving wheels in the current frame.

[0177] For each frame, the above steps are repeated to achieve navigation.

[0178] The vehicle navigation system uses four 3D LiDARs (MID360) distributed at the four corners of the chassis as information source sensors, achieving two functions:

[0179] 1. When the chassis is outside the car door, it can automatically align with the center line of the car door and adjust its posture.

[0180] 2. When the chassis (or robot) is inside the car, it can move in a straight line along the center line of the left and right car walls.

[0181] This application uses multiple three-dimensional radar sensors to collect environmental point cloud data, and realizes spatial alignment and fusion of multi-source point clouds by calibrating the external parameters of each radar. After obtaining high-quality fused point clouds, the system further performs plane detection and navigation path extraction, and finally calculates vehicle control instructions in combination with posture data. This application divides the visual interface into three parts. The left side displays the spliced ​​current frame environmental point cloud, the middle displays the pre-processed point cloud of the key focus area, and the right side displays the path judgment results and provides calculation results such as the linear speed and turning angle of the two active wheels. The key calculation process of the present invention will be explained in detail step by step below.

[0182] It should be noted that the specific embodiments are merely explanations and illustrations of the technical solutions of the present invention and cannot be used to limit the scope of protection. Any minor changes made based on the claims and description of the present invention shall still fall within the scope of protection of the present invention.

Claims

1. A navigation method for a high-speed unloading robot for cartons, characterized in that The following steps are involved: For each carton working surface, perform the following steps: Step 1: Use multi-radar point cloud stitching and frame generation technology to obtain a fused point cloud frame P fuse (t); Step 2: Point cloud P fuse (t) Perform filtering to obtain the point cloud P; Step 3: Based on the point cloud P, and using the RANSAC algorithm, determine the angle difference Δθ between the ideal orientation of the robot and the current orientation of the robot, as well as the distance difference v between the ideal position of the robot and the current position of the robot in the x and y directions. x 、v y ; Step 4: Use Δθ, v x and v y As input, and through rigid body motion translation, determine the linear velocity v of the robot's active wheel in the robot coordinate system i and the position P of the driving wheel in the robot coordinate system i ; Step 5: Repeat steps 1 to 4 to achieve robot navigation.

2. The navigation method for a high-speed unloading robot for cartons in a van according to claim 1 is characterized in that The step 1 is specifically as follows: Step 1: Obtain the pose transformation matrix of the radar relative to the robot coordinate system through external parameter calibration; Step 1 and 2: Use the pose transformation matrix to construct the rigid body transformation T of the radar sensor i ; Step 13: Use the radar sensor to collect the original point cloud and convert it into a homogeneous vector Then use the homogeneous vector Combined with rigid body transformation T i , get the coordinates of the point cloud in the robot coordinate system; Step 14: Set the refresh cycle of the radar sensor, and perform rigid body transformation registration on the point cloud collected within the refresh cycle, and then fuse them to obtain the point cloud P fuse (t).

3. The navigation method for a high-speed unloading robot for cartons according to claim 2 is characterized in that The step 2 is specifically as follows: Step 21: Use voxel grid method to quantize point cloud P fuse Perform downsampling; Step 22: Use the x, y, and z axes in the robot coordinate system and the set boundary threshold to perform the down-sampled point cloud P. fuse Perform cropping and retain the point cloud within the cropped range; Step 2 and 3: Construct a Kd-Tree model based on the retained point cloud, and obtain the k point clouds closest to each point cloud based on the Kd-Tree model. Then, based on the Euclidean distance between this point cloud and each point cloud in the k point clouds, obtain the average Euclidean distance d corresponding to the point cloud. i ; Step 24: According to the average Euclidean distance d corresponding to each point cloud i , take the average value, and get the average value Step 25: According to the average Euclidean distance d i and average Calculate the standard deviation σ, and remove outliers based on the standard deviation σ and the set threshold to obtain the point cloud P.

4. The navigation method for a high-speed unloading robot for cartons in a van according to claim 3 is characterized in that In step 21, the voxel grid method is used to map the point cloud P fuse Downsampling is expressed as: V(u,v,w)=[ul,(u+1)l)×[vl,(v+1)l)×[wl,(w+1)l) Among them, V(u,v,w) is the voxel index obtained after dividing the three-dimensional space, u,v,w are the integer grid numbers of the coordinate axis directions of x,y,z, respectively, c uvw is the centroid of all points falling into the voxel V(u,v,w), q is the coordinate vector of an original point cloud point within the voxel V(u,v,w), and l is the length of the voxel side.

5. The navigation method for a high-speed unloading robot for cartons according to claim 4 is characterized in that The step three is specifically as follows: For the same carton working surface, first determine the Δθ and v obtained in the previous frame y , whether it satisfies v y Less than 0.01, and Δθ less than 1°. If not satisfied, the robot is adjusted to align with the carriage opening. Otherwise, the robot is adjusted to follow the center line of the carriage inner wall. If the current frame is the initial frame, the robot is adjusted to align with the carriage opening. The steps to adjust the robot's carriage opening alignment are: Step A1: Perform a straight-through filter on the point cloud P in the x-direction to obtain the point cloud C, which is expressed as: in, The maximum and minimum values ​​in the x-direction preset for carriage opening alignment; Step A2: Use the RANSAC algorithm to extract the target plane model in point cloud C. RANSAC randomly selects three points each time to construct a spatial plane and calculates (n, d), where n is the unit normal vector of the current plane and d is the offset from the current plane to the origin. Based on the constraint conditions, traverse the entire point cloud to obtain the target plane, that is, the plane of the car door when the cartons are fully loaded. The constraint conditions are expressed as: inlier(n,d)={p∈C||n·p+d∣≤δ},|inlier(n,d)||<points Among them, δ is the maximum distance from a point in the plane to the fitting plane, points is the minimum number of point clouds in the fitting plane, and ε is the angle between the normal vector of the fitting plane and the positive half axis of the x-axis of the vehicle coordinate system; Step A3: Normalize the normal vector of the target plane and find the closest point c0 to the origin on the plane. c0 is expressed as: c0=-dn Where a, b, c, and d are the coefficients of the plane equation formula, ax+by+cz+d=0; Step A4: Extract the points in the target plane, obtain the extreme values ​​of the point cloud coordinates in the y and z directions, and define the ROI boundary to surround the extreme value range. Then obtain the center point c of the ROI area and project the ROI normal vector on the XY plane to obtain the vector d xy , expressed as: Among them, c 0,x Indicates the x-coordinate value of point c0, n x ,n y Represent the components of the unit normal vector n in the x and y directions respectively; Step A5: Based on the result of step A4, determine the angle difference Δθ between the ideal orientation of the robot and the current orientation of the robot, and the distance difference v between the ideal position of the robot and the current position of the robot in the x and y directions. x 、v y , expressed as: Among them, θ IMU is the current orientation of the robot, c x ,c y are the coordinate values ​​of the center point c in the x and y directions respectively, and τ is the threshold of the distance from the robot coordinate origin to the carriage entrance in the x direction; The steps for adjusting the robot to follow the center line of the inner wall of the carriage are as follows: Step B1: Perform straight-through filtering on the point cloud P in the x and z directions to obtain the point cloud S, which is expressed as: in, They are the maximum and minimum values ​​in the x and z directions when walking along the center line of the inner wall of the carriage. Step B2: Divide the point cloud S into the left point cloud S according to the coordinates in the y direction l With the right point cloud S r Then, the RANSAC algorithm is used to extract the fitting planes n on the left and right sides of the carriage wall. L and n R , and calculate the unit normal vector, n L and n R Expressed as: Among them, a L ,b L ,c L ,d L ,a R ,b R ,c R ,d R is the coefficient of the plane equation formula, a L x+b L y+c L z+d L =0,a R x+b R y+c R z+d R =0; Step B3: Project the unit normal vector onto the XY plane to obtain the horizontal truncation and Among them, n L,x ,n L,y ,n R,x ,n R,y n L ,n R Components in the x and y directions; Step B4: Cut off horizontally and Get the midline tangent vector d raw , expressed as: d raw (d x ,d y ,0)=n l ×n r ; Among them, d x d y are the midline tangent vectors d raw Components in the x and y directions; Step B5: According to the midline tangent vector d raw , get the midline tangent unit vector d corr , expressed as: Step B6: According to n L 、n R d L and d R , get the closest point p to the origin on the left and right planes L 、p R and their geometric midpoint positions p mid , expressed as: p L =-d L n L p R =-d R n R Step B7: Based on d corr and p mid , introduce the smoothing coefficient α, and get the tangent unit vector of the current frame and the geometric midpoint position of the left and right planes of the current frame Expressed as: in, and is the tangent unit vector of the midline of the previous frame and the geometric midpoint position of the left and right planes; Step B8: According to and Determine the angle difference Δθ between the ideal robot orientation and the current robot orientation, and the distance difference v between the ideal robot position and the current robot position in the x and y directions. x 、v y , expressed as: in, are vectors The components in the x and y directions, ∈, are the preset robot forward speeds. for The coordinate value in the y direction.

6. The navigation method for a high-speed unloading robot for cartons according to claim 1 is characterized in that The linear velocity v of the driving wheel of the robot in the robot coordinate system i Expressed as: Among them, x i 、y i are the coordinates of the current driving wheel in the x and y directions, respectively, and ω is the angular velocity of rotation regulated according to Δθ.

7. The navigation method for a high-speed unloading robot for cartons according to claim 6 is characterized in that The position P of the driving wheel in the robot coordinate system i Expressed as:

8. The navigation method for a high-speed unloading robot for cartons according to claim 7 is characterized in that The robot is equipped with four wheels, which are symmetrically distributed. The left front wheel and the right rear wheel are active wheels. The steering wheel angle and linear speed of the left front wheel and the right rear wheel are expressed as: Among them, W and L are the lengths of the driving wheel symmetrical distribution rectangle in the x and y directions respectively, θ F ,θ R are the absolute encoding angles of the left and right driving wheels in the current frame, s F ,s R They are respectively the linear velocities of the left and right driving wheels in the current frame.

9. The navigation method for a high-speed unloading robot for cartons in a van according to claim 2 is characterized in that The rigid body transformation T i Expressed as: Among them, R i is the rotation amount, R i =R z (ψ i )R y (θ i )R x (φ i ), R z (ψ i ), R y (θ i ), R x (φ i ) are the rotation amounts relative to the z, y, and x axes, respectively, and t i is the translation, SE(3) represents the 3D special Euclidean group.

10. The navigation method for a high-speed unloading robot for cartons in a van according to claim 2 is characterized in that The refresh period of the radar sensor is 200ms.

Images