Highly repeatable indoor global positioning method based on laser inertial navigation SLAM and beacon positioning

Abstract

The present application discloses a high-repeatability indoor global positioning method based on laser inertial navigation SLAM and beacon positioning, which belongs to the field of robot positioning technology, including S10, determining the pose transformation matrix of a temporary fixed coordinate system, building a map, and S20, obtaining in real time the position and posture of the AGV car when it is relocated when running in a scene, wherein in an indoor environment with high repeatability, acoustic beacon positioning is used to serve as indoor GPS positioning, and the point cloud sampling of the laser radar is marked with key frames to complete downsampling and feature extraction, calling NDT and ICP algorithms and performing iterative calculation of point cloud alignment to obtain the transformation matrix of the first key frame from the vehicle coordinate system to the world coordinate system, and in subsequent key frames, the key frame is used as the initial value, and the aforementioned algorithm is called again to obtain the position and posture of the AGV car in the subsequent key frames, which can ensure the rapidity of iterative calculation and the real-time and accuracy of global positioning, meet the requirements of high-precision real-time global positioning, and have high robustness.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a high-repeatability indoor global positioning method based on laser inertial navigation SLAM and beacon positioning, which is used for agricultural robots to achieve repositioning in high-repeatability agricultural greenhouse sites with existing mapping information. Background Art

[0002] With the continuous development of global agricultural production, agricultural greenhouses play an increasingly important role in modern agriculture. Agricultural greenhouses provide a controlled environment that can optimize crop growth conditions, improve yield and quality, and reduce dependence on natural resources. However, the complexity of the greenhouse environment and the lack of effective navigation and positioning solutions limit the application of agricultural robots in greenhouses.

[0003] At present, agricultural robots are widely used in agricultural production, such as planting, watering, fertilizing and picking. In order to achieve autonomous agricultural production, agricultural robots need to accurately perceive and understand the environment in the greenhouse and be able to perform high-precision positioning and navigation in complex scenarios.

[0004] LiDAR sensors have been widely used in various laser SLAM frameworks and solutions due to the density of data points, adaptability to complex environmental conditions, and high precision. As for binocular cameras, depth cameras, etc., they are also widely used in various visual SLAM frameworks and solutions due to their low deployment cost and ideal results in a good environment. The laser or vision-based relocalization problem is essentially a registration problem that compares sensor information with map information. When faced with scenes with a high degree of repetition, a series of iterative registration algorithms are greatly affected by the initial value of the iteration, and it is very easy to iterate to a local optimal solution rather than the required global optimal solution, resulting in very limited results in the use of LiDAR or cameras to achieve relocalization.

[0005] On the other hand, the use of agricultural greenhouses also puts forward higher requirements for the selection and use of sensors. Agricultural greenhouses are indoor scenes with complex internal ambient light conditions. The existing GPS global positioning system is affected by signals in indoor scenes, and its sensors are difficult to work properly and cannot provide instant and effective global location information. The visual camera is also limited by ambient light, and the front-end images collected are difficult to use for subsequent analysis and feature extraction.

[0006] After searching the prior art, it was found that Chinese patent document number CN114526739A, published on May 24, 2022, discloses a method for indoor repositioning of a mobile robot, a computer device and a product, which is a method for realizing global positioning of a mobile robot in the same field. This method extracts topological semantic features from the global map and the lidar frame data respectively to obtain a geometric-topological semantic feature map and a topological semantic scan, and performs feature matching and optimization on the two based on the triangular topological structure to obtain the registration result between the two, thereby obtaining the global posture of the robot. For complex scenes, this technology uses topological feature extraction to simplify the point cloud map and lidar frame scan data, thereby reducing the difficulty of implementing the point cloud registration algorithm, and completes the registration through similarity analysis based on triangular topological structure. However, for scenes in agricultural greenhouses with high repetitiveness, the topological semantic feature map extracted by topological features also has high repeatability. In the similarity analysis based on triangular topological structure, the problem of local optimum and global optimum still exists. Summary of the invention

[0007] One advantage of the present invention is that it provides a high-repeatability indoor global positioning method based on laser inertial navigation SLAM and beacon positioning, which is used for agricultural robots to achieve repositioning in high-repeatability agricultural greenhouse sites with existing mapping information, wherein a number of acoustic beacons are deployed in the scene, and the distance is calculated based on the beacons to obtain the global position of the AGV car, and it is used as the initial position value and passed to the ICP repositioning algorithm. At the same time, the SLAM algorithm based on laser inertial navigation is used to complete the scene map information for mapping, and the point cloud information after downsampling and feature extraction of each frame of the laser radar is used, and the ICP algorithm is used for iterative calculation, so that the global positioning algorithm can still avoid falling into the problem of local optimality in the face of scenes with high repetition, so that accurate global posture can be obtained, and the robustness of the algorithm can be effectively improved.

[0008] The present invention provides a high-repeatability indoor global positioning method based on laser inertial navigation SLAM and beacon positioning, which is used for an agricultural robot to achieve repositioning in a high-repeatability agricultural greenhouse site with existing mapping information, wherein the positioning method comprises the following steps:

[0009] S10, determining the posture transformation matrix of the temporary fixed coordinate system and constructing a map, specifically including:

[0010] S11. Set the transformation matrix from the acoustic beacon coordinate system to the world coordinate system as the unit matrix, deploy the AGV car at any position and posture, start the AGV car, start the laser inertial navigation SLAM, start bag recording in the terminal, where SLAM publishes frame timestamps through topics in the first key frame and the second key frame of the map, where the world coordinate system is a coordinate system customized in the agricultural greenhouse and bound to the agricultural greenhouse, where the acoustic beacon coordinate system is a coordinate system bound to the agricultural greenhouse and where the position of the AGV car is given in the acoustic beacon positioning system;

[0011] S12, obtain the timestamp closest to the two timestamps under the Marvelmind topic in the bag, obtain the acoustic beacon coordinates of the two key frames, obtain the coordinates of the two key frames in the mapping coordinate system under the SLAM / mapping topic in the bag, obtain the coordinate transformation matrix from the mapping coordinate system to the acoustic beacon coordinate system, and publish the obtained transformation matrix to the corresponding topic, where the two timestamps are the first key frame timestamp and the second key frame timestamp, and the two key frames are the first key frame and the second key frame;

[0012] S20, real-time acquisition of the relocated position and posture of the AGV when it is running in the scene, specifically including:

[0013] S21. Obtain the map point cloud data, real-time laser radar point cloud data, real-time acoustic beacon coordinate data, and the transformation matrix between the world coordinate system, the acoustic beacon coordinate system, and the mapping coordinate system in the moving path of the AGV, where the mapping coordinate system is the coordinate system used by the environmental point cloud map built in the process of calling SLAM for mapping;

[0014] S22, calling in real time to relocate the position of the AGV in the acoustic beacon coordinate system at the first key frame and the second key frame, and performing coordinate transformation on the position to obtain the position of the AGV in the mapping coordinate system;

[0015] S23, assuming that the AGV moves along the x-axis direction from the first relocation key frame to the second relocation key frame, defining the coordinate system that completely coincides with the vehicle coordinate system where the AGV is located and fixed in the mapping coordinate system at the second relocation key frame as a temporary fixed coordinate system, constructing a transformation matrix from the mapping coordinate system to the temporary fixed coordinate system for the second relocation key frame, and obtaining a four-dimensional transformation matrix from the vehicle coordinate system to the mapping coordinate system under the second relocation key frame;

[0016] S24. For subsequent repositioning keyframes, the transformation matrix of the previous repositioning keyframe is used as a reference, and the translation and rotation of the AGV between the previous repositioning keyframe and the current repositioning keyframe are multiplied to obtain the four-dimensional transformation matrix of the AGV from the vehicle coordinate system to the mapping coordinate system, thereby obtaining the real-time 6-DOF posture of the AGV in the world coordinate system.

[0017] According to one embodiment of the present invention, in step S11, after the AGV is started, the AGV is first maintained to move in a straight line in the forward direction for a predetermined time, and then the AGV is controlled to move freely in the environment, wherein the predetermined time refers to the time for completing the recording and processing of the first key frame and the second key frame in the mapping process.

[0018] According to an embodiment of the present invention, in step S12, it is set that at the first key frame time and the second key frame time of the mapping process, the position coordinates of the AGV in the acoustic beacon coordinate system are A and B respectively. 1，M =(x1, y1, 0), A 2M =(x2, y2, 0), set the coordinates of the AGV car in the mapping coordinate system at the first key frame and the second key frame in the SLAM / mapping topic in the bag to A 1，m , A 2，m , according to the SLAM mapping principle, A 1，m =(0,0,0), where when the map is started, the AGV runs in the forward direction, A 2，m =(x, 0, 0), where according to the spatial coordinate transformation, the acoustic beacon coordinate system and the mapping coordinate system satisfy the following transformation:

[0019]

[0020] Thus, the coordinate transformation matrix from the mapping coordinate system to the acoustic beacon coordinate system is obtained.

[0021] , where t M is the translation vector in the coordinate transformation from the mapping coordinate system to the acoustic beacon coordinate system, is the rotation matrix in the coordinate transformation from the mapping coordinate system to the acoustic beacon coordinate system, where

[0022]

[0023] According to an embodiment of the present invention, a calculation accuracy parameter e is introduced, according to A 1，M and A 2，M , If e is within 0.15, it is considered that the position and posture of the AGV at the first key frame in the mapping process meet the predetermined requirements. Otherwise, the initial position of the AGV is replaced, and the mapping and repositioning are recalculated.

[0024] According to an embodiment of the present invention, in step S22, the position of the AGV in the acoustic beacon coordinate system at the time of the first key frame and the second key frame is set to B 1，M (x1, y1, 0), B 2，M (x2, y2, 0), the coordinate transformation from the acoustic beacon coordinate system to the mapping coordinate system is performed. At the first key frame and the second key frame in the relocalization process, the position of the AGV in the mapping coordinate system is B 1，m , B 2，m ;

[0025]

[0026] At the moment of repositioning the first key frame and the moment of repositioning the second key frame, the positions of the AGV in the temporarily overlapping temporary fixed coordinate system are:

[0027]

[0028] B 2，l =(0,0,0),

[0029] in is: the coordinate transformation matrix from the temporary fixed coordinate system to the mapping coordinate system, The coordinate transformation matrix from the vehicle coordinate system to the mapping coordinate system at the second key frame of the relocalization process is obtained to estimate the four-dimensional transformation matrix from the mapping coordinate system to the vehicle coordinate system under the second key frame of the relocalization process. The rotation matrix from the vehicle coordinate system to the mapping coordinate system has the following form: Based on B 1，m and B 2，m The equations listed obtain the values ​​of cosθ2 and sinθ2; the four-dimensional transformation matrix is ​​estimated Substitute the initial value into the ICP algorithm to calculate the four-dimensional transformation matrix of the AGV from the vehicle coordinate system to the mapping coordinate system under the second key frame of relocation Where K 2，v It is the single-frame point cloud data obtained by the laser radar scanning on the AGV car, where K m This is the point cloud data of the AGV car under the second key frame of relocalization.

[0030] According to an embodiment of the present invention, in step S24, the translation and rotation of the AGV between the last repositioning key frame and the current repositioning key frame are defined as ΔT, which is obtained from the data recorded by the IMU sensor. The data recorded by the IMU sensor includes the average speed v=(v, 0, 0) and the average angular velocity ω=(0, 0, ω) of the AGV and the time interval Δt between the last repositioning key frame and the current repositioning key frame;

[0031] The four-dimensional transformation matrix of the AGV car in the real-time 6-DOF posture in the world coordinate system is: in K represents the four-dimensional transformation matrix from the vehicle coordinate system to the mapping coordinate system under the relocation key frame i; i，v K represents the single-frame point cloud data obtained by the laser radar scanning on the AGV car at the i-th key frame moment; m It is the mapping point cloud data of the AGV car under the i-th key frame of relocalization.

[0032] According to an embodiment of the present invention, in step S24, before calculating the real-time 6-DOF position of the AGV in the world coordinate system, a four-dimensional transformation matrix is ​​first estimated. in Represents the coordinate transformation rotation matrix from the vehicle coordinate system to the mapping coordinate system under the relocation key frame i; R z (ωΔt) represents the rotation matrix corresponding to the rotation around the z-axis with ωΔt as the angle, wherein the four-dimensional transformation matrix of the AGV vehicle in the real-time 6-DOF posture in the world coordinate system is obtained by substituting the four-dimensional transformation matrix estimation performed first into the ICP algorithm for calculation.

[0033] According to one embodiment of the present invention, the calculated 6-DOF posture of the AGV car is broadcast and data transmitted using tf, and the relocated position and posture of the car when running in the agricultural greenhouse scene are displayed in real time on the Rviz interface.

[0034] Beneficial effect: The high-repeatability indoor global positioning method based on laser inertial navigation SLAM and beacon positioning provided in the present application, by deploying several acoustic beacons in the scene, measuring the distance based on the beacons, obtaining the global position of the AGV car, and passing it as the initial position value to the ICP repositioning algorithm, while using the laser inertial navigation-based SLAM algorithm to complete the scene map information for mapping, and the point cloud information after downsampling and feature extraction of each frame of the laser radar, and using the ICP algorithm for iterative calculation, can enable the global positioning algorithm to avoid falling into the problem of local optimality in the face of scenes with high repetition, thereby being able to obtain accurate global pose, while also being able to effectively improve the robustness of the algorithm.

[0035] These and other objects, features and advantages of the present invention will be fully reflected in the following detailed description. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 A schematic diagram showing different coordinate systems and their transformations in the greenhouse scene of the present application is shown.

[0037] Figure 2 A schematic diagram of the process of SLAM lidar tightly coupled mapping in this application is shown.

[0038] Figure 3 A schematic diagram of the process of relocating and acquiring the position and posture of the AGV vehicle in this application is shown. DETAILED DESCRIPTION

[0039] The following description is used to disclose the present invention so that those skilled in the art can implement the present invention. The preferred embodiments described below are only examples, and those skilled in the art can think of other obvious variations. The basic principles of the present invention defined in the following description can be applied to other embodiments, variations, improvements, equivalents, and other technical solutions that do not deviate from the spirit and scope of the present invention.

[0040] Those skilled in the art should understand that, in the disclosure of the specification, the orientation or position relationship indicated by the terms "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc. are based on the orientation or position relationship shown in the drawings, which are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the above terms should not be understood as limiting the present invention.

[0041] It is to be understood that the term "one" should be understood as "at least one" or "one or more", that is, in one embodiment, the number of an element may be one, while in another embodiment, the number of the element may be multiple, and the term "one" should not be understood as a limitation on the quantity.

[0042] refer to Figures 1 to 3 According to a preferred embodiment of the present invention, a high-repeatability indoor global positioning method based on laser inertial navigation SLAM and beacon positioning will be described in detail below, wherein the high-repeatability indoor global positioning method based on laser inertial navigation SLAM and beacon positioning is used for an agricultural robot to achieve repositioning in a high-repeatability agricultural greenhouse with existing mapping information, wherein the agricultural robot is implemented as an AGV car, or an AGV smart car, which is equipped with:

[0043] 1. Livox mid-70 LiDAR sensor, which can obtain LiDAR scan point clouds of the surrounding environment in real time;

[0044] 2. IMU sensor can measure the real-time acceleration of the AGV and estimate the mileage and speed of the AGV through integral calculation;

[0045] 3. MarvelMind acoustic beacon positioning system, through the beacons or beacon sensors deployed at various locations in the environment, measures the distance from the AGV to each beacon in real time, and calculates the position coordinates of the AGV in the environment based on this. Its position coordinates belong to the acoustic beacon coordinate system;

[0046] 4. ROS control system.

[0047] The size of the AGV car is approximately 1023mm×778mm×788mm. Its working environment and the application environment of the algorithm are a single-layer agricultural greenhouse with a size of 110m×85m. It is rectangular, with a marker placed every 12m along the length and every 4m along the width. The spatial repetition is high. At the same time, the ground of the agricultural greenhouse is horizontal, and an acoustic beacon positioning tool is deployed near each of the four corners of the agricultural greenhouse, and its precise position is known.

[0048] The computer of the AGV car is installed with the Ubuntu operating system, Python dependency library and ROS robot operating system software. It is also installed with the GTSAM factor graph optimization framework and Livox mid-70 hardware driver. The hardware is connected to the Livox mid-70 laser radar sensor, and the SLAM algorithm, relocalization algorithm and Marvelmind work package are also deployed.

[0049] Specifically, the high-repeatability indoor global positioning method comprises the following steps:

[0050] S10, determining the posture transformation matrix of the temporary fixed coordinate system and constructing a map, specifically including:

[0051] S11. First, set the origin (or zero point) and positive direction of the world coordinate system in the agricultural greenhouse environment, and test the origin and positive direction of the acoustic beacon positioning system. In this example, the origin of the two coordinate systems is set to the upper left corner of the map, and the x and y directions are respectively along the length and width of the map, and the two coordinate systems are completely consistent. Set the transformation matrix from the acoustic beacon coordinate system to the world coordinate system is the identity matrix I, At the same time, the AGV car is deployed in any position and posture, and the AGV car is started, the laser inertial navigation SLAM is started, and the bag recording is started in the terminal. At the same time, the SLAM is turned off after the map is built, the bag recording is ended, and the recording data is stored, so that the relevant map files can be checked in the specified path, wherein the SLAM publishes the frame timestamp through the topic in the first key frame and the second key frame of the map, wherein the world coordinate system (world, referred to as w) is a coordinate system customized in the agricultural greenhouse and bound to the agricultural greenhouse, wherein the acoustic beacon coordinate system (Marvelmind, referred to as M) is a coordinate system bound to the agricultural greenhouse and given in the acoustic beacon positioning system where the position of the AGV car is located;

[0052] S12, obtain the timestamp closest to the two timestamps under the Marvelmind topic in the bag, obtain the acoustic beacon coordinates of the two key frames, and obtain the coordinates of the two key frames in the mapping coordinate system under the SLAM / mapping topic in the bag, and then obtain the coordinate transformation matrix from the mapping coordinate system to the acoustic beacon coordinate system, and publish the obtained transformation matrix to the corresponding topic to complete the map construction of the AGV car operating environment, where the two timestamps are the first key frame timestamp and the second key frame timestamp, and the two key frames are the first key frame and the second key frame;

[0053] S20, real-time acquisition of the relocated position and posture of the AGV when it is running in the scene, specifically including:

[0054] S21. Obtain the map point cloud data, real-time laser radar point cloud data, real-time acoustic beacon coordinate data, and the transformation matrix between the world coordinate system, the acoustic beacon coordinate system, and the mapping coordinate system in the moving path of the AGV, where the mapping coordinate system (map, referred to as m) is the coordinate system used by the environmental point cloud map built in the process of calling SLAM for mapping;

[0055] S22, calling in real time to relocate the position of the AGV in the acoustic beacon coordinate system at the first key frame and the second key frame, and performing coordinate transformation on the position to obtain the position of the AGV in the mapping coordinate system;

[0056] S23, assuming that the AGV moves along the x-axis direction from the first relocation key frame to the second relocation key frame, defining the coordinate system that completely coincides with the vehicle coordinate system (vehicle, referred to as v) where the AGV is located and fixed in the mapping coordinate system at the second relocation key frame as the temporary fixed coordinate system, constructing a transformation matrix from the mapping coordinate system to the temporary fixed coordinate system for the second relocation key frame, and obtaining a four-dimensional transformation matrix from the vehicle coordinate system to the mapping coordinate system under the second relocation key frame;

[0057] S24. For subsequent repositioning key frames, the transformation matrix of the previous repositioning key frame is used as a reference, and the translation and rotation of the AGV car between the previous repositioning key frame and the current repositioning key frame are multiplied to obtain the four-dimensional transformation matrix of the AGV car from the vehicle coordinate system to the mapping coordinate system, and then the real-time 6-DOF posture of the AGV car in the world coordinate system is obtained. For example, for the fourth repositioning key frame, the transformation matrix of the third repositioning key frame is used as a reference, and the translation and rotation of the AGV car between the third repositioning key frame and the fourth repositioning key frame are multiplied to obtain the four-dimensional transformation matrix of the AGV car from the vehicle coordinate system to the mapping coordinate system in the fourth repositioning key frame, that is, the current 6-DOF posture of the AGV car in the world coordinate system, that is, the real-time position and posture information of the AGV car.

[0058] The vehicle coordinate system (also called the lidar coordinate system. Since the lidar and the vehicle are fixedly installed, the two are essentially one coordinate system, referred to as vehicle), that is, the coordinate system that is always relatively fixed with the AGV during the movement of the AGV. Each frame of point cloud data collected by the lidar is presented in the vehicle coordinate system of the AGV. At the first key frame of mapping, the vehicle coordinate system completely coincides with the mapping coordinate system.

[0059] Therefore, the present application proposes a global positioning algorithm in a high-repeatability environment based on the SLAM algorithm of laser inertial navigation and the acoustic beacon positioning method. It uses the acoustic beacon positioning system to act as a pseudo "GPS" positioning solution in a known indoor environment, returns the global position estimate of the car, and marks the point cloud sampling of the laser radar with key frames at agreed time intervals, and completes downsampling and feature extraction. At the same time, in the repositioning first key frame, the global position estimate of the car is used as the initial value, and the NDT algorithm and ICP algorithm in the library are called in sequence. The first key frame point cloud and the map point cloud are iteratively calculated for point cloud registration, and the transformation matrix from the vehicle coordinate system (or the laser radar coordinate system on the vehicle) to the world coordinate system in the first key frame is obtained, that is, the 6-DOF posture of the car; in the second key frame, the third key frame and other subsequent key frames, the posture transformation matrix obtained in the previous key frame is used as the initial value, and the NDT algorithm and ICP algorithm are called in sequence to obtain the transformation matrix from the laser radar coordinate system to the world coordinate system in this frame, thereby obtaining the position and posture of the AGV car in any subsequent key frame. In the test of local data sets, the relocalization algorithm can ensure the rapidity of iterative calculation and the real-time performance of global positioning at a registration speed of 1 keyframe / s. Its ICP point cloud registration error score is within 0.05, which is the same order of magnitude as the ICP registration error score of relocalization using SLAM mapping data sets (i.e., relocalization starting from a completely correct initial pose, where positioning can be completed in only one iteration). When the acoustic beacon positioning method is not called, the point cloud registration of NDT and ICP is performed from the initial value of 0. Due to the high repetitiveness of the environment, the system is very likely to fall into the local optimum, and the two point cloud registration algorithms cannot avoid the local optimum through iteration, resulting in global positioning errors. Its ICP point cloud registration score is about 0.3, and the error will gradually accumulate as the AGV runs, which cannot meet the positioning requirements. Therefore, this project effectively improves the robustness of the relocalization algorithm in a high-repetitive environment by combining various methods including acoustic beacon positioning, and can meet the requirements of high-precision real-time global positioning.

[0060] In one embodiment, in step S11, after the AGV is started, the AGV is first maintained to move in a straight line in the forward direction for a predetermined time, such as 10 seconds or 15 seconds, and then the AGV is controlled to move freely in the environment, wherein the predetermined time refers to the time for recording and processing the first key frame and the second key frame in the mapping process, so that after the mapping is completed, the AGV can process data more quickly and effectively in the subsequent relocation process. When the algorithm processes the first key frame and the second key frame data, it is preset that the laser radar moves in a straight line between the two key frames, but in fact there is a certain deviation between the movement of the AGV and the preset straight line movement. At the same time, the greater the deviation, the greater the deviation between the transformation matrix estimation and the true value obtained by the relocation algorithm from the vehicle coordinate system to the mapping coordinate system, which leads to a longer iteration time of the point cloud registration algorithm, and there is a possibility of error. Therefore, after starting the AGV, it is best to make the AGV walk in the forward direction for 10-15s to avoid the above problems.

[0061] In one embodiment, in step S12, it is set that at the first key frame time and the second key frame time of the mapping process, the position coordinates of the AGV in the acoustic beacon coordinate system are A and 1，M =(x1, y1, 0), A 2，M =(x2, y2, 0), and set the coordinates of the AGV car in the mapping coordinate system at the first key frame and the second key frame in the SLAM / mapping topic in the bag to A 1，m , A 2，m , according to the SLAM mapping principle, A 1，m =(0, 0, 0). At the same time, since the AGV runs in the forward direction when the map is built, A 2，m =(x, 0, 0), according to the spatial coordinate transformation, the acoustic beacon coordinate system and the mapping coordinate system satisfy the following transformation:

[0062]

[0063] Thus, the coordinate transformation matrix from the mapping coordinate system to the acoustic beacon coordinate system can be obtained: , where t M is the translation vector in the coordinate transformation from the mapping coordinate system to the acoustic beacon coordinate system, is the rotation matrix in the coordinate transformation from the mapping coordinate system to the acoustic beacon coordinate system, where

[0064] ;

[0065] In the process of obtaining, A 1，M and A2，M Subtracting the two equations yields: Expanding and simplifying the matrix equation, we can obtain the following two equations:

[0066]

[0067] And in In this paper, θ1 is an undetermined parameter.

[0068] In addition, considering that there may be measurement errors in the actual measurement process of sensors, including lidar sensors and acoustic beacon sensors, the AGV's travel distance in the mapping coordinate system and its travel distance in the acoustic beacon coordinate system are not equal when it moves from the first key frame of mapping to the second key frame of mapping. Therefore, as a preferred method, the calculation accuracy parameter e is introduced. 1，M and A 2，M , If e is within 0.15, it is considered that the position and posture of the AGV car at the first key frame in the mapping process meet the predetermined requirements, and all the acquired data are acceptable, which can ensure that the final relocated position and posture have better accuracy. Otherwise, the initial position of the AGV car is replaced, and the mapping and relocation are recalculated.

[0069] In one embodiment, in step S22, the position of the AGV in the acoustic beacon coordinate system at the time of the first key frame and the second key frame is set to B 1，M (x1, y1, 0), B 2，M (x2, y2, 0), and at the same time, the coordinate transformation from the acoustic beacon coordinate system to the mapping coordinate system is performed. At the first key frame and the second key frame in the relocalization process, the position of the AGV in the mapping coordinate system is B 1，m , B 2，m ;

[0070]

[0071]

[0072] Specifically, based on the above four equations, the position information B of the AGV in the acoustic beacon coordinate system can be obtained based on the relocation of the first key frame and the relocation of the second key frame. 1，M , B 2，M Find the position information B of the AGV car in the map coordinate system at the first key frame of relocation and the second key frame of relocation 1，m =(x′1,y′1,0),B 2，m =(x′2, y′2, 0);

[0073] By default, the AGV moves along the x-axis from the first key frame of relocation to the second key frame of relocation. At the first key frame of relocation and the second key frame of relocation, the vehicle coordinate system and the mapping coordinate system of the AGV completely overlap. Therefore, it can be determined that at the first key frame of relocation and the second key frame of relocation, the positions of the AGV in the temporarily overlapping temporary fixed coordinate systems are:

[0074]

[0075] B 2，l =(0,0,0);

[0076] Thus, a transformation matrix from the mapping coordinate system to the temporary fixed coordinate system can be constructed for the repositioned second key frame, and the transformation matrix is ​​the same as the transformation matrix from the mapping coordinate system to the vehicle coordinate system;

[0077] in is: the coordinate transformation matrix from the temporary fixed coordinate system to the mapping coordinate system, The coordinate transformation matrix from the vehicle coordinate system to the mapping coordinate system at the second key frame of the relocalization process is: The rotation matrix from the vehicle coordinate system to the mapping coordinate system has the following form: Based on B 1，m and B 2，m The equations listed obtain the values ​​of cosθ2 and sinθ2, where Obviously, cos 2 θ2+sin 2 θ2=1; the four-dimensional transformation matrix is ​​estimated Substituting it into the ICP algorithm as the initial value, the four-dimensional transformation matrix of the AGV from the vehicle coordinate system to the mapping coordinate system under the second key frame of relocation can be calculated. Where K 2，v It is the single-frame point cloud data obtained by the laser radar scanning on the AGV car, where K m This is the point cloud data of the AGV car under the second key frame of relocalization.

[0078] In the ICP algorithm, the ICP algorithm first calculates the translation change, and then calculates the rotation change. Since the correct translation change and relatively accurate rotation change have been given in this relocation method, the initial value of the ICP algorithm is already near the global optimum, and the first step (calculating the translation change) has reached the optimum, so the ICP algorithm will iteratively calculate according to the rotation change and give the four-dimensional transformation matrix under the global optimal solution.

[0079] In one embodiment, in step S24, the translation and rotation of the AGV between the last repositioning key frame and the current repositioning key frame are defined as ΔT, which is obtained from the data recorded by the IMU sensor. The data recorded by the IMU sensor includes the average speed v=(v, 0, 0) and the average angular velocity ω=(0, 0, ω) of the AGV and the time interval Δt between the last repositioning key frame and the current repositioning key frame;

[0080] The four-dimensional transformation matrix of the AGV car in the real-time 6-DOF posture in the world coordinate system is: in K represents the four-dimensional transformation matrix from the vehicle coordinate system to the mapping coordinate system under the relocation key frame i; i，v K represents the single-frame point cloud data obtained by the laser radar scanning on the AGV car at the i-th key frame moment; m It is the mapping point cloud data of the AGV car under the i-th key frame of relocalization.

[0081] Since the time interval between two frames is short, in step S24, the transformation matrix from the mapping coordinate system to the vehicle coordinate system changes due to the movement of the AGV between two adjacent frames. That is, before calculating the real-time 6-DOF pose of the AGV in the world coordinate system, a 4D transformation matrix estimation is first performed. in Represents the coordinate transformation rotation matrix from the vehicle coordinate system to the mapping coordinate system under the relocation key frame i; R z (ωΔt) represents the rotation matrix corresponding to the rotation around the z-axis with ωΔt as the angle, wherein the four-dimensional transformation matrix of the AGV vehicle in real-time 6-DOF posture in the world coordinate system is obtained by substituting the four-dimensional transformation matrix estimation performed first into the ICP algorithm calculation, thereby further improving the registration efficiency and accuracy of the ICP algorithm.

[0082] In one embodiment, the calculated 6-DOF position of the AGV is broadcast and transmitted using tf, and the relocated position and posture of the vehicle when it is running in the agricultural greenhouse scene are displayed in real time on the Rviz interface.

[0083] It is worth mentioning that the NDT algorithm extracts map block features by analyzing the distribution parameters (mean and variance) of the point cloud in each map block after dividing the large-scale point cloud map into blocks, and compares the parameters of the lidar with the map block features to achieve rough positioning. In the greenhouse interior scene, since the map size is relatively small, the repetition between each map block is high, and there are few independent features, the NDT algorithm steps are not very effective.

[0084] Although the ICP iterative algorithm can calculate the variance by comparing point by point, obtain the coordinate transformation matrix, first obtain the coordinate translation transformation, and then obtain the coordinate rotation transformation, its registration accuracy is relatively high, but because it is an iterative algorithm, its default transformation matrix is ​​the unit matrix. In the face of the high repetitive environment in the greenhouse, if the iteration direction near the initial value is selected as the local optimum rather than the global optimum, it is easy to fall into the local optimum and give an erroneous result;

[0085] The acoustic beacon can provide the real-time position information of the AGV in the acoustic beacon coordinate system. In the mapping part, the coordinate transformation matrix between the acoustic beacon coordinate system and the mapping coordinate system is obtained through the above method; therefore, in the relocation part, the acoustic beacon can provide the real-time position information of the vehicle in the mapping coordinate system;

[0086] In addition, during the repositioning process, the marking frequency of the key frames can be set to mark at time intervals, such as 1 second intervals. At the same time, this parameter can also be adjusted in the source code to adjust the time interval.

[0087] It should be noted that the terms "first, second" in the present invention are only used for descriptive purposes and do not indicate any order. They cannot be understood as indicating or implying relative importance. These terms can be interpreted as names.

[0088] It should be understood by those skilled in the art that the embodiments of the present invention described above and shown in the accompanying drawings are only examples and do not limit the present invention. The advantages of the present invention have been fully and effectively achieved. The functional and structural principles of the present invention have been demonstrated and explained in the embodiments, and the embodiments of the present invention may be deformed or modified in any way without departing from the principles.

Claims

1. A high-repeatability indoor global positioning method based on laser inertial navigation SLAM and beacon positioning is used for agricultural robots to achieve repositioning in high-repeatability agricultural greenhouses with existing mapping information, characterized in that: The positioning method comprises the following steps: S10, determining the posture transformation matrix of the temporary fixed coordinate system and constructing a map, specifically including: S11. Set the transformation matrix from the acoustic beacon coordinate system to the world coordinate system as the unit matrix, deploy the AGV car at any position and posture, start the AGV car, start the laser inertial navigation SLAM, start bag recording in the terminal, where SLAM publishes frame timestamps through topics in the first key frame and the second key frame of the map, where the world coordinate system is a coordinate system customized in the agricultural greenhouse and bound to the agricultural greenhouse, where the acoustic beacon coordinate system is bound to the agricultural greenhouse and is the coordinate system where the position of the AGV car is given in the acoustic beacon positioning system; S12, obtain the timestamp closest to the two timestamps under the Marvelmind topic in the bag, obtain the acoustic beacon coordinates of the two key frames, obtain the coordinates of the two key frames in the mapping coordinate system under the SLAM / mapping topic in the bag, obtain the coordinate transformation matrix from the mapping coordinate system to the acoustic beacon coordinate system, and publish the obtained transformation matrix to the corresponding topic, where the two timestamps are the first key frame timestamp and the second key frame timestamp, and the two key frames are the first key frame and the second key frame; S20, real-time acquisition of the relocated position and posture of the AGV when it is running in the scene, specifically including: S21. Obtain the map point cloud data, real-time laser radar point cloud data, real-time acoustic beacon coordinate data, and the transformation matrix between the world coordinate system, the acoustic beacon coordinate system, and the mapping coordinate system in the moving path of the AGV, where the mapping coordinate system is the coordinate system used by the environmental point cloud map constructed in the process of calling SLAM for mapping; S22, calling in real time to relocate the position of the AGV in the acoustic beacon coordinate system at the first key frame and the second key frame, and performing coordinate transformation on the position to obtain the position of the AGV in the mapping coordinate system; S23, assuming that the AGV moves along the x-axis direction from the first relocation key frame to the second relocation key frame, defining the coordinate system that completely coincides with the vehicle coordinate system where the AGV is located and fixed in the mapping coordinate system at the second relocation key frame as a temporary fixed coordinate system, constructing a transformation matrix from the mapping coordinate system to the temporary fixed coordinate system for the second relocation key frame, and obtaining a four-dimensional transformation matrix from the vehicle coordinate system to the mapping coordinate system under the second relocation key frame; S24. For subsequent repositioning keyframes, the transformation matrix of the previous repositioning keyframe is used as a reference, and the translation and rotation of the AGV between the previous repositioning keyframe and the current repositioning keyframe are multiplied to obtain the four-dimensional transformation matrix of the AGV from the vehicle coordinate system to the mapping coordinate system, thereby obtaining the real-time 6-DOF posture of the AGV in the world coordinate system.

2. The high-repeatability indoor global positioning method according to claim 1, characterized in that: In step S11, after the AGV is started, the AGV is first maintained to move in a straight line in the forward direction for a predetermined time, and then the AGV is controlled to move freely in the environment, wherein the predetermined time refers to the time for completing the recording and processing of the first key frame and the second key frame in the mapping process.

3. The high-repeatability indoor global positioning method as claimed in claim 2, characterized in that: In step S12, it is set that at the first key frame time and the second key frame time of the mapping process, the position coordinates of the AGV in the acoustic beacon coordinate system are A and 1，M =(x1, y1, 0), A 2，M =(x2, y2, 0), set the coordinates of the AGV car in the mapping coordinate system at the first key frame and the second key frame in the SLAM / mapping topic in the bag to A 1，m , A 2，m , according to the SLAM mapping principle, A 1，m =(0,0,0), where when the map is started, the AGV runs in the forward direction, A 2，m =(x, 0, 0), where according to the spatial coordinate transformation, the acoustic beacon coordinate system and the mapping coordinate system satisfy the following transformation: Thus, the coordinate transformation matrix from the mapping coordinate system to the acoustic beacon coordinate system is obtained. , where t M is the translation vector in the coordinate transformation from the mapping coordinate system to the acoustic beacon coordinate system, is the rotation matrix in the coordinate transformation from the mapping coordinate system to the acoustic beacon coordinate system, where , in the process of obtaining, A 1，M and A 2，M Subtracting the two equations yields: Expanding and simplifying the matrix equation yields the following two equations: And in In this paper, θ1 is an undetermined parameter.

4. The high-repeatability indoor global positioning method as claimed in claim 3, characterized in that: Introduce the calculation accuracy parameter e, according to A 1，M and A 2，M , If e is within 0.15, it is considered that the position and posture of the AGV at the first key frame in the mapping process meet the predetermined requirements. Otherwise, the initial position of the AGV is replaced, and the mapping and repositioning are recalculated.

5. The high-repeatability indoor global positioning method as claimed in claim 4, characterized in that: In step S24, the translation and rotation of the AGV between the last relocation key frame and the current relocation key frame are defined as ΔT, which is obtained from the data recorded by the IMU sensor. The data recorded by the IMU sensor include the average speed v=(v,0,0) and the average angular velocity ω=(0,0,ω) of the AGV and the time interval Δt between the last relocation key frame and the current relocation key frame; The four-dimensional transformation matrix of the AGV car in the real-time 6-DOF posture in the world coordinate system is: in K represents the four-dimensional transformation matrix from the vehicle coordinate system to the mapping coordinate system under the relocation key frame i; i,v K represents the single-frame point cloud data obtained by the laser radar scanning on the AGV car at the i-th key frame moment; m It is the mapping point cloud data of the AGV car under the i-th key frame of relocalization.

6. The high-repeatability indoor global positioning method according to claim 5, characterized in that: In step S24, before calculating the real-time 6-DOF position of the AGV in the world coordinate system, a four-dimensional transformation matrix is ​​first estimated. in Represents the coordinate transformation rotation matrix from the vehicle coordinate system to the mapping coordinate system under the relocation key frame i; R z (ωΔt) represents the rotation matrix corresponding to the rotation around the z-axis with ωΔt as the angle, wherein the four-dimensional transformation matrix of the AGV vehicle in the real-time 6-DOF posture in the world coordinate system is obtained by substituting the four-dimensional transformation matrix estimation performed first into the ICP algorithm for calculation.

7. The high-repeatability indoor global positioning method according to claim 6, characterized in that: The calculated 6-DOF posture of the AGV car is broadcast and transmitted using tf, and the relocated position and posture of the car when running in the agricultural greenhouse scene are displayed in real time on the Rviz interface.

Images