CN113218384B - Indoor AGV self-adaptive positioning method based on laser SLAM - Google Patents

Abstract

The invention relates to an indoor AGV self-adaptive positioning method based on laser SLAM, and relates to the technical field of positioning by fusing a plurality of sensors of laser SLAM, a speedometer and an IMU. The indoor AGV self-adaptive positioning system comprises an upper computer, a lower computer, a motor driving device, a 2D laser radar, a odometer and an inertia measuring unit. The method has the advantages that the two-dimensional grid map is constructed by the aid of the laser radar to the working environment of the AGV, self-adaptive switching is performed between the ICP matching algorithm based on the feature points and the matching algorithm based on optimization according to different structural features in the working environment, accordingly, positioning accuracy and speed of the AGV in the complex working environment are improved, and the problems of low positioning accuracy, poor positioning flexibility and high cost of the AGV in the prior art can be effectively solved.

Description

An adaptive positioning method for indoor AGV based on laser SLAM

technical field

The invention relates to the technical field of robot positioning, in particular to an adaptive switching laser SLAM positioning method of multi-sensor fusion of 2D laser radar, odometer and inertial measurement unit.

Background technique

With the rapid development of society, more and more people are shopping online in recent years, which makes the logistics industry develop towards a more intelligent and efficient direction. An Automated Guided Vehicle (AGV) is a vehicle that automatically travels along a designated path in a work environment, usually used for the transportation of goods in industrial production or warehouses. It is one of the most widely used robots in the logistics industry. AGVs play an increasingly important role in modern industrial production and have been widely used.

As a car with autonomous positioning and navigation functions, AGV is a complex system integrating environmental perception, path planning, intelligent decision-making and control. In the research on various related technologies of autonomous mobile robots, positioning technology is one of the key technologies in autonomous navigation, and its positioning accuracy directly affects the execution of AGV's subsequent mapping and navigation tasks. Therefore, how to ensure that the AGV can be positioned quickly and accurately is a technical difficulty, which has always attracted the attention of researchers.

The commonly used positioning methods of AGV are tape positioning, two-dimensional code positioning, and inertial navigation positioning. Although these positioning methods have high positioning accuracy and are simple and easy to implement, they have poor positioning flexibility. When the working environment changes, the positioning equipment needs to be re-laid. Add extra cost.

The existing 2D laser SLAM localization algorithms still have certain defects and cannot provide consistent and stable localization information. However, feature point-based ICP and optimization-based matching algorithms have different advantages and disadvantages and can complement each other. Therefore, in order to improve the positioning accuracy and robustness of the AGV, the present invention proposes an adaptive switching positioning algorithm based on the Sigmoid function, which switches between the improved ICP matching algorithm and the optimization-based matching algorithm. The angle value of the environment and the channel width and narrowness value during operation are set as the switching threshold conditions, so as to improve the positioning accuracy of the AGV in the complex working environment.

SUMMARY OF THE INVENTION

In view of the above problems, the purpose of the present invention is to provide an indoor AGV adaptive positioning method based on laser SLAM, that is, firstly use 2D laser radar to establish a grid map of the working environment, and then use the adaptive switching method on the established map Switch between feature point-based ICP and optimization-based matching algorithms.

In order to achieve the above object, the present invention adopts the following technical solutions:

An indoor AGV adaptive positioning method based on laser SLAM, including 2D lidar, odometer, inertial measurement unit, NVIDIA TX2 embedded development board, STM32 microcontroller and remote monitoring terminal;

The 2D lidar is connected to the NVIDIA TX2 host computer through a USB cable, the lidar scans the environmental information, and transmits each frame of scanned data to the host computer for processing;

The inertial measurement unit (IMU) uses a USB-TTL adapter cable to connect with the host computer NVIDIA TX2 to collect the body attitude information of the AGV;

The odometer of the AGV is connected to the STM32 microcontroller through the GPIO interface. The odometer collects the AGV's running mileage and driving speed information, and transmits the collected information to the STM32 lower computer;

The upper computer NVIDIA TX2 and the lower computer STM32 are connected bidirectionally through the serial port, the motor drive module of the AGV is connected through the IO port of the STM32 microcontroller, and the upper computer NVIDIA TX2 uses the local area network to communicate with the remote monitoring terminal;

The STM32 microcontroller of the lower computer transmits the collected speed and mileage information of the AGV odometer to the upper computer through the serial port, and accepts the AGV motor drive information sent by the upper computer;

The 2D lidar will collect the obstacle information in the working environment, the inertial measurement unit (IMU) will collect the attitude angle value of the AGV body, and the STM32 microcontroller will transfer the collected AGV speed and mileage information to the NVIDIATX2 host computer. , By using the laser SLAM algorithm, firstly map the working environment of the AGV, control the AGV to move and establish a two-dimensional grid map;

The remote monitoring terminal is used to monitor the running status of the AGV and publish the coordinate information of the target position;

AGV is a differential AGV, including motor drive circuit, left-wheel drive motor and right-wheel drive motor. The upper computer issues running commands to the lower computer, and then the lower computer issues drive commands to control the rotation of the left and right wheel motors of the AGV to control the running state of the AGV. ;

The host computer is NVIDIA's TX2 high-performance embedded development board, equipped with Linux and ROS operating systems. The development board has a multi-core processor and powerful performance, which can be used for the operation of laser SLAM algorithm and data processing, as a running platform for algorithm transplantation;

The upper computer, lower computer, drive motor, 2D lidar, inertial measurement unit and odometer use the power carried by the AGV itself to supply power after conversion;

The AGV in the present invention is mainly used in industrial production and logistics warehouse environments, and most obstacles are static obstacles, including walls, production equipment, storage shelves, and placed goods.

The present invention also provides an indoor AGV adaptive positioning algorithm based on laser SLAM, the steps are as follows:

Step 1, on the remote monitoring terminal, manually control the AGV to move in the working environment, and use the laser SLAM algorithm to construct a two-dimensional grid map for the environment;

Step 2: First, input the pose information of the AGV target point on the remote monitoring terminal, and use the path planning algorithm in the navigation algorithm to plan an optimal running route;

Step 3, the present invention defines an improved Sigmoid function for the adaptive switching matching algorithm, and the function is defined as follows:

In formula (1), θ is the angle value of the curve environment, λ is the width value of the running channel, the value of θ is obtained after scanning the surrounding environment with lidar, and the value of λ can be directly obtained by using the scanning data of lidar ;

因此可通过f(λ,θ)的值来确定AGV运行时采用的匹配算法；When formula (1)

Therefore, the matching algorithm used when the AGV is running can be determined by the value of f(λ, θ);

When the AGV starts to run, the data of the inertial measurement unit and the lidar are collected synchronously. The angle value θ of the curve environment is obtained after scanning the surrounding environment by the lidar; The frame data is preprocessed, and the outliers with large changes in the distance value are filtered out. Finally, the average λ of the channel width is calculated, and the values of θ and λ are substituted into formula (1) for calculation. value to judge the environmental state of the AGV at the current moment;

Step 4, when the calculated value of f(λ, θ) is greater than 0, it means that the AGV is in a wide environment at this time, and an optimization-based matching algorithm is used. This method is suitable for large-scene environments, such as AGV in industrial production. Loading and unloading area, cargo transfer area in logistics warehouse;

The idea of the algorithm based on optimization is to give an objective function, and transform the inter-frame matching problem of lidar into the problem of solving the extremum of the objective function. The definition of the objective function is as follows:

In formula (2), T=(T _x , T _y , T _θ ) is the pose of the robot in the (x, y, z) coordinate system, S _i (T) represents the i-th frame of the lidar The data are coordinately transformed with the pose T, and M(x) represents the occupancy probability of the transformed coordinate x in the map;

In formula (2),

M(S _i (T)) is a nonlinear function, and formula (2) is first-order Taylor expansion to obtain formula (3)

For a linear system, take the derivative of ΔT in equation (3) and set it equal to 0 to obtain equation (4)

Solving formula (4) can obtain ΔT, as shown in formula (5):

通过对地图进行双线性插值进行求解。令T＝T+ΔT，不断进行迭代，直到满足精度要求或者到达最大迭代次数求解出机器人的位姿(x,y,θ)。In formula (5),

Solve by bilinear interpolation of the map. Let T=T+ΔT, and iterate continuously until the accuracy requirements are met or the maximum number of iterations is reached to solve the pose (x, y, θ) of the robot.

Through the above algorithm, the pose of the AGV in a wide environment is obtained, providing high-precision positioning data for navigation;

Step 5. When the AGV enters the curve environment, the front-end matching algorithm of the laser SLAM will have a matching error due to the problems of wheel slippage and lidar scanning data point coverage, which will affect the positioning accuracy of the AGV;

When the calculated value of f(λ, θ) is less than 0, it means that the AGV is in the environment of a curve or a shelf at this time. Therefore, the ICP matching algorithm based on feature points is used to improve the matching accuracy by extracting the feature points in the environmental structure. and speed, thereby improving the positioning accuracy of AGV;

Use lidar to scan the environmental structure, and extract feature points by preprocessing the data, as shown in Equation (6) and Equation (7):

为所选取的特征点同一帧数据中，特征点左侧范围内点集的计算值，公式(7)中

为所选取的特征点同一帧数据中，特征点右侧范围内点集的计算值；In formula (6)

is the calculated value of the point set within the range to the left of the feature point in the same frame of data of the selected feature point, in formula (7)

is the calculated value of the point set within the right range of the feature point in the same frame data of the selected feature point;

Substitute Equation (6) and Equation (7) into Equation (8) as follows:

According to formula (8), formula (9) and formula (10), the points within the range on both sides of the point that may be the feature point are fitted with a straight line, and the angle between the two fitted straight lines is calculated. If the calculated f The larger the value of (k) is, the closer the angle between the two fitted straight lines is to 90 degrees, the point is the required feature point. If the value of f(k) is smaller, the more deviation from 90 degrees is. Eliminate points that are not feature points;

Through the calculation of step 5, the feature points that meet the requirements in the environmental structure are extracted;

Step 6, through the feature points in the environment structure extracted in step 5, because only the feature points in the environment are extracted for matching, compared with the point-by-point matching rule adopted by the original algorithm, the matching speed and accuracy of the algorithm can be improved. . First, establish the kinematic model of AGV, use the odometer to estimate the pose of AGV in the environment through the kinematic model, and then fuse the information obtained by lidar feature matching and IMU observation to calculate the pose of AGV in the actual environment. The calculated actual pose information is used to update the estimated AGV pose, thereby obtaining the exact pose (x, y, θ) of the AGV. Using the feature point-based ICP matching algorithm can improve the matching speed and matching accuracy of the algorithm, thereby reducing the positioning error of the AGV when running in a curved environment;

Step 7. When the AGV runs from a wide area to the shelf area, due to the particularity of the shelf structure, it has many feature points that can be used and can be easily extracted. Therefore, when the AGV runs to a narrow area such as a shelf, the feature point-based method is adopted. The ICP matching algorithm can also improve the positioning accuracy;

The lidar mounted on the AGV is used to collect the obstacle information on both sides. When the AGV enters the narrow shelf area, f(λ, θ) is less than 0, so the matching algorithm is switched to the ICP matching algorithm based on feature points. Step (5) Matching with the method of step (6) by extracting the feature points of the shelf, so as to obtain the AGV pose data with higher accuracy;

Compared with the positioning method adopted by the existing AGV, the adaptive positioning method based on laser SLAM adopted in the present invention has high positioning flexibility, good adaptability and low cost, which can meet the needs of most application environments, and adopts ROS-based development. The system reduces the difficulty of development and improves the efficiency of algorithm debugging.

Description of drawings

FIG. 1 is a hardware structure diagram of an AGV adaptive positioning method based on laser SLAM in the present invention.

FIG. 2 is a flowchart of an AGV adaptive positioning algorithm based on laser SLAM in the present invention.

Detailed ways

The technical solutions of the present invention are further described below with reference to the accompanying drawings and through specific embodiments.

FIG. 1 is a hardware structure diagram of an AGV adaptive positioning method based on laser SLAM in the present invention, as shown in FIG. 1 .

In the present invention, the host computer selects NVIDIA TX2 high-performance embedded development board, which is equipped with Ubuntu16.04 and ROS Kinetic operating system. The lower computer is an STM32 embedded development board, and the remote monitoring terminal is a laptop computer, which is also equipped with Ubuntu and ROS operating systems, which are used to display the running status of the AGV and issue target point coordinate commands.

The RPLidar A2 lidar used is connected to the host computer through a USB cable. The lidar collects the distance and angle information of obstacles within the effective range, and transmits the scanned information to the host computer for processing.

The inertial measurement unit (IMU) used is connected to the host computer with a USB-TTL cable, which is used to collect the rotation angle information of the vehicle body when the AGV is running, and directly transmit the collected information to the host computer, which can improve the calculation efficiency of the algorithm. Avoid the time delay brought by connecting with the lower position and then transmitting it to the upper computer and the data error brought by the transmission.

The adopted odometer is one-way connected to the lower computer through the GPIO interface, which is used to collect the linear speed, angular velocity and running mileage information of the AGV, and the collected information is processed and transmitted to the upper computer through the lower computer.

In this positioning method, the upper computer and the lower computer are connected bidirectionally through the serial line, the lower computer is connected with the motor drive module of the AGV through the IO port, and the remote monitoring terminal and the upper computer are connected through the local area network.

The remote monitoring terminal publishes the position information of the target point to be reached by the AGV, receives the command through the local area network host computer, uses the lidar to collect the obstacle position information in the environment, and simultaneously collects the information of the IMU and odometer, and transmits these data to the host computer. , using the laser SLAM algorithm to calculate the pose data of the AGV in real time, and transmit it to the navigation algorithm part. target point for movement.

FIG. 2 is a flowchart of an indoor AGV adaptive positioning algorithm based on laser SLAM in the present invention. It can be seen from FIG. 2 that the present invention provides an AGV adaptive positioning algorithm based on laser SLAM, and the steps are as follows:

Step 1: First, through the remote monitoring terminal, control the AGV to construct a two-dimensional grid map of the working environment.

Step 2: The remote monitoring terminal transmits the position information of the target point to be reached by the AGV to the upper computer.

Step 3: The host computer receives the target point location information issued by the remote monitoring terminal, uses the current position of the AGV as the starting point, and uses the path planning algorithm to plan an optimal path to the target point on the two-dimensional grid map.

Step 4: When the AGV moves towards the target point, the navigation algorithm needs the real-time pose data of the AGV running in the environment. Only high-precision positioning data can ensure that the AGV reaches the target point accurately. Therefore, the present invention adopts an adaptive The positioning algorithm is used to improve the positioning accuracy of AGV. The positioning algorithm process is as follows:

Step 4.1, when the AGV receives the operation command issued by the remote monitoring terminal, the host computer collects the sensor detection data carried by the AGV, and substitutes the processed θ and λ values into formula (1) for calculation. If the calculated f(λ) ,θ) value is less than 0, indicating that the AGV is in a curved or narrow operating environment at this time, and the host computer switches the matching algorithm to the ICP algorithm based on feature points, which can make full use of the environmental structural features for matching, and can also avoid the AGV being in a Matching error caused by wheel slippage in the curve environment, thereby improving the positioning accuracy and speed of the AGV;

Step 4.2, if the calculated value of f(λ, θ) is greater than 0, it means that the AGV is running in a wide environment at this time, and the host computer will switch the matching algorithm to an optimization-based algorithm to ensure that the AGV operates in a large-scene environment. positioning accuracy;

Step 5: Determine whether the AGV has reached the target position point. If it reaches the target position point, the upper computer sends a stop operation command to the lower computer, and the lower computer sends a stop operation command to the motor drive module of the AGV, and waits for the next operation command to be issued; If the target point is not reached, go back to step 4, and the AGV continues to move until it reaches the target point and stops running and waits for the next command.

Claims (3)

1. An indoor AGV self-adaptive positioning method based on laser SLAM is characterized in that: the method comprises the following steps:

s1, firstly, constructing a two-dimensional grid map for the working environment of the AGV by using a laser radar, an inertia measurement unit and a milemeter which are carried by the AGV;

s2, releasing coordinate information of a target point on the two-dimensional grid map on the remote monitoring terminal;

s3, drawing an optimal path to a target point by using a navigation algorithm with the current position of the AGV as a starting point;

s4, the AGV runs according to the planned path, the position and posture data of the AGV in the working environment are obtained in real time by the aid of the upper computer, and the running state of the AGV is displayed on the remote monitoring terminal;

s5, collecting data of the carried inertia measurement unit and the laser radar in real time in the running process of the AGV, substituting the collected data values into an improved Sigmoid function after processing, judging the working environment of the AGV at the current time according to the calculated function value so as to adopt a corresponding positioning algorithm, adopting an ICP (inductively coupled plasma) matching algorithm based on characteristic points if the AGV enters a curve or a narrow shelf area, and adopting an optimized matching algorithm if the AGV enters a wide running environment;

the Sigmoid function modified in step S5 is defined as follows:

in the formula (1), theta is an angle value of a curve environment, lambda is a width value of an operation channel, the theta value is obtained by scanning the surrounding environment by using a laser radar and processing, the lambda value is directly obtained by using scanning data of the laser radar, and in the formula (1)

Determining a matching algorithm adopted by the AGV during operation according to the value of f (lambda, theta);

when the AGV starts to operate, synchronously acquiring data of an inertia measurement unit and a laser radar, wherein a curve environment angle value theta is obtained by scanning the surrounding environment through the laser radar and processing; preprocessing the first few frames of data of the laser radar, filtering outliers with large distance value changes, solving an average value lambda of the channel width, substituting values of theta and lambda into a formula (1) for calculation, and judging the environment state of the AGV at the current moment according to the calculated value;

s6, calculating the position data of the AGV in real time through the upper computer to judge whether the AGV runs to the target point, when the AGV runs to the target point, sending a running stopping instruction by the upper computer, and controlling a driving motor to stop the AGV at the target point by the lower computer after receiving the instruction; otherwise, continuing to execute the step 5 until the AGV reaches the target point and stops running.

2. The indoor AGV self-adaptive positioning method based on the laser SLAM as claimed in claim 1, wherein the upper computer is an NVIDIA TX2 high performance embedded development board equipped with Linux and ROS operating systems, and the lower computer is an STM32 embedded development board.

3. The adaptive indoor AGV positioning method based on laser SLAM of claim 1,

the laser radar is connected with the upper computer through a USB line, and obstacle position information around the AGV is transmitted to the upper computer;

the Inertial Measurement Unit (IMU) is directly connected with an upper computer through a USB-TTL line, and angle and angular velocity information of the AGV is collected;

the odometer is connected with the lower computer through a GPIO interface to acquire the running mileage information of the AGV;

the AGV motor driving module is connected with the lower computer through an IO port and controls the AGV to move according to a planned path;

the upper computer is connected with the lower computer through a serial port, the lower computer transmits the odometer information to the upper computer, and the upper computer issues an operation instruction to the lower computer through the serial port;

the upper computer is connected with the remote monitoring terminal through the local area network, transmits the real-time pose of the AGV to the remote monitoring terminal and receives a control instruction issued by the remote monitoring terminal, and the remote monitoring terminal is used for issuing pose information of a target point and monitoring the running state of the AGV.

Images