Research on SLAM and Path Planning Method of Inspection Robot in Complex Scenarios

1. Introduction

With the introduction of "Industry 4.0", robotics technology has rapidly advanced. Among them, inspection robots have been extensively used in aerospace, manufacturing, agriculture, service industries, and other fields due to their superior flexibility, mobility, and functionality [1,2]. As inspection application scenarios become more diversified and complex, higher requirements are placed on the autonomous navigation performance of robots. Robot navigation technology mainly consists of SLAM technology and path planning technology. Simultaneous Localization and Mapping (SLAM) is the process by which a mobile robot determines its own position and creates a map through sensors carried in the surrounding environment. Path planning technology creates the optimal navigation path for the robot to reach the target location based on different task goals and requirements.

SLAM technology can be classified into two categories based on different sensors: vision-based and LIDAR-based. The MonoSLAM method proposed by Davison et al. estimates the camera pose by extracting sparse feature points frame by frame, which is the first real-time visual SLAM system using a single camera [3]. Subsequently, Klein et al. proposed the PTAM method, which introduced nonlinear optimization and keyframe mechanism [4], solving the problem of high computational complexity in MonoSLAM. Newcombe et al. proposed a dense per-pixel method based on RGB-D camera, which can achieve real-time tracking and reconstruction [5]. As for LIDAR-based SLAM methods, Grisetti et al. proposed a 2D SLAM algorithm based on particle filtering, which solved the problem of particle dissipation caused by resampling through reducing the number of particles [6]. Konolige et al. proposed the first open-source algorithm based on graph optimization by using highly optimized and non-iterative square-root factorization to sparsify and decouple the system [7]. Kohlbrecher et al. designed the Hector SLAM algorithm, which matches the current frame’s LIDAR data with the factor graph and optimizes the pose using the Gauss-Newton method to obtain the optimal solution and bias [8].

Path planning is to plan a collision-free optimal path from the starting point to the destination point in the map environment. Path planning can be divided into global and local path planning according to whether the environment information is known in advance. Dijkstra et al. proposed the shortest path planning algorithm, which uses breadth-first search to search for paths [9]. Hart et al. proposed the A-star algorithm, which reduces the search nodes by using heuristic evaluation function and improves the efficiency of path searching [10]. Fox et al. proposed the DWA, which samples velocity dynamically in the robot’s sampling space according to the robot’s kinematic model and current motion parameters, and selects the best trajectory [11]. To address the insufficient evaluation function of DWA, Chang et al. proposed an improved DWA algorithm based on Q-learning, which modifies and extends the evaluation function and adds two evaluation functions to improve navigation performance and achieve higher navigation efficiency and success rate in complex unknown environments [12]. Rösmann et al. proposed the Time Elastic Band algorithm based on multi-objective optimization, which ensures that the robot can output a smooth trajectory under the premise of satisfying its kinematic constraints [13].

Both lidar and camera sensors are essential for factory inspections, but their applicability depends on the specific application scenarios and factory environments. Lidar is suitable for long-distance and high-precision measurements, such as inventory management in large warehouses and position control in robot operations. However, lidar has limitations in processing details and colors, making it unsuitable for high-visual requirements scenes. Camera sensors, with their functions such as image recognition, detection, and tracking, are ideal for environments that require high-precision visual detection and recognition, such as detecting product dimensions, shapes, colors, and performing automated visual inspections during assembly processes on production lines. In addition, camera sensors have strong processing capabilities for details and colors, providing more detailed image information. However, camera sensors are highly dependent on ambient light, which can limit their effectiveness in poor lighting conditions or when obstacles are present.

In this article, we designed two sets of SLAM systems based on multi-line lidar and vision sensors respectively to meet the inspection needs of robots in different environments. Global path planning using the A-star algorithm was adopted to improve navigation efficiency and help robots quickly plan the optimal path, reducing inspection time and costs. However, the A-star algorithm is not suitable for dynamic environments. Therefore, we introduced the Time Elastic Band algorithm, a real-time path planning algorithm that can adapt to changes in the environment and obstacles, resulting in more optimized path planning results to improve the efficiency of robot inspections and reduce collisions and other safety issues between robots and factory equipment. The method proposed in this article adopts a global and local fusion path planning algorithm for both lidar SLAM and vision SLAM, ultimately achieving autonomous navigation and obstacle avoidance of inspection robots in comple.

Figure 1. System framework.

Figure 1. System framework.

2. Inspection robot SLAM system

2.1. Visual-SLAM Algorithm Design and Implementation

In the factory environment, the position and motion of inspection robots and equipment may undergo rapid changes, thus requiring real-time acquisition and processing of sensor data for accurate localization and mapping. To achieve this, we have chosen an appearance-based localization and mapping method that is invariant to time and scale, as shown in the structural diagram in Figure 2.

From Figure 2, it can be seen that RGB-D images are used as external input, and the ORB algorithm is used to extract feature points from RGB-D images [14]. Then, a Bag-of-Words based image matching method is used to match feature points between adjacent frames. The loop-closure detection mechanism is introduced to eliminate drift. When the robot returns to an area previously visited, the loop-closure detection can identify the area and match the newly observed data with the previous map data, thus addressing the cumulative drift problem. Next, graph optimization is performed, where the robot’s poses are represented as nodes in the graph and the observed data is represented as edges. Then, the least squares method is used to optimize the positions of all nodes, minimizing the error between observed and predicted data. Finally, a dense map can be generated.

This vision-based mapping and localization algorithm utilizes global and local loop-closure detection techniques, which can identify and handle errors and drift in sensor data, improving the robustness and accuracy of localization and mapping, and enhancing the efficiency and real-time performance of inspection robots.

2.2. Multi-line LiDAR-based SLAM Algorithm Design and Implementation

In indoor factory environments, lighting conditions can vary with time and location, which may lead to misidentification of objects or inaccurate positioning by visual sensors. However, laser sensors do not require an external light source, as they emit their own laser beams and are not affected by lighting issues. Therefore, we have adopted a lightweight and ground optimized laser odometry and mapping method, whose structural diagram is shown in Figure 3. Firstly, the laser point cloud data is dimensionally reduced, projecting the 3D laser onto a 2D depth image, segmenting the ground according to the pitch angle, clustering non-ground point clouds, and obtaining labeled point cloud data. Then, feature extraction is performed based on smoothness, resulting in four sets of feature point clouds. Constraint relationships are established for the feature point cloud sets, and the 6-DOF pose transformation matrix is solved using the Levenberg-Marquardt (LM) optimization method. Subsequently, loop detection is conducted using the Iterative Closest Point (ICP) algorithm, and finally, the current point cloud is mapped to the global map based on graph optimization, completing the establishment of a high-precision map [15].

1) Point cloud segmentation: Due to the complexity of the inspection environment and other factors, noise may exist in the laser point cloud data. We first use point cloud segmentation to filter out noise. By projecting a frame of 3D point cloud onto a 2D depth image using a projection method, ground segmentation is performed to separate non-ground points [16]. Let $P_t=p_1,p_2,\dots ,p_n$ be the point cloud data obtained by the lidar at time t, where $P_i$ is a point in $P_t$. These points are projected onto a depth image, and the 3D points in space become 2D pixels in space. After projection, the Euclidean distance $r_i$ of point $P_i$ to the sensor is obtained. Since the 3D point cloud contains a large amount of ground information, it is necessary to filter the point cloud to improve the efficiency and accuracy of feature extraction. Firstly, the ground points are labeled, and the labeled ground points will no longer be segmented in subsequent steps. After separating ground points and non-ground points, the non-ground points are processed by clustering. After this module, each point has its own segmentation label (ground or non-ground), row and column indices in the depth image, and the Euclidean distance $r_i$ to the sensor.

2)Feature extraction: According to the smoothness, the projected depth image is horizontally divided into several sub-images. For each sub-image, the following process is performed [17]: Let S be the set of continuous points in the same row in the depth image, and calculate the smoothness c of point $P_i$.

$$c=\frac{1}{\left|S\right|·∥r_i∥}∥\sum _{j\in S,j\ne i}\left(r_j-r_i\right)∥$$

(1)

where $r_i$ and $r_j$ are the Euclidean distances from points $P_i$ and $P_j$ to the sensor. According to Equation (1), the smoothness of each point can be calculated, and then the smoothness is sorted. After sorting, feature points are selected. Different types of features are segmented based on the set threshold $c_{th}$. The edge points with a smoothness c greater than $c_{th}$ are selected as set $F_{me}$, and the plane points with a smoothness c less than $c_{th}$ are selected as set $F_{mp}$. The largest $n_{F_{me}}$ edge points with the maximum c value and the smallest $n_{F_{mp}}$ plane points with the minimum c value are selected from all sub-images to form the edge feature point set $F_{me}$ and the plane feature point set $F_{mp}$. Then, $n_{F_e}$ edge points not belonging to ground points with the maximum c value are selected from set $F_{me}$ to form set $F_e$, and $n_{F_p}$ plane points belonging to ground points with the minimum c value are selected from set $F_{mp}$ to form set $F_p$. Obviously $F_e\subset F_{me}\&F_p\subset F_{mp}$.

3)Radar Odometry: The odometry module estimates the robot’s pose change between adjacent frames using a radar sensor. In the estimation process, tag matching is used to narrow down the matching range and improve accuracy, and a two-step LM optimization method is used to find the transformation relationship between two consecutive frames [18]. The first step uses ground feature points $F_p$ to obtain $\left[t_z,t_{roll},t_{pitch}\right]$, and the second step matches the edge features extracted from the segmented point cloud to obtain the transformation $\left[t_x,t_y,t_{yaw}\right]$. Finally, by fusing $\left[t_z,t_{roll},t_{pitch}\right]$ and $\left[t_x,t_y,t_{yaw}\right]$, a 6-DOF pose transformation matrix $\left[t_x,t_y,t_z,t_{roll},t_{pitch},t_{yaw}\right]$ is obtained.

4)Radar Mapping: After obtaining the pose change between adjacent frames with radar odometry, the features in the feature set $F_{me}^t,F_{mp}^t$ at time t are matched with the surrounding point cloud $Q^{t-1}$ to further refine the pose transformation. Then, using the LM optimization, the final transformed pose is obtained and the pose graph is sent to GTSAM for map optimization to update the sensor estimated pose and the current map [19].

In addition, noise may exist in the collected laser point cloud data. In order to achieve high-precision localization on the map, it is necessary to preprocess the high-precision map. We use a statistical-based robust filter to remove outliers, a pass-through filter to clip the point cloud within a specified coordinate range, and a voxel grid filter to downsample the point cloud. For the inspection task that requires high-precision real-time localization, we perform real-time localization on the constructed high-precision map through point cloud registration. First, the reference point cloud (i.e., high-precision map) is transformed into a multivariate normal distribution [20]. If the transformation parameters can accurately match the reference point cloud and the current point cloud, the transformed points in the reference frame have a high probability density. Therefore, an optimization method can be used to calculate the transformation parameters that maximize the sum of the probability densities. In this case, the two sets of laser point cloud data match best. The specific algorithm steps are as follows:

1) Given the current scanning point cloud S and the reference point cloud T, the space occupied by the T point cloud is divided into voxel grids of a specified size, and the expected vector $\overrightarrow{\mu }$ and covariance matrix $\Sigma$ of N points in each voxel grid are calculated.

$$\overrightarrow{\mu }=\frac{1}{N}\sum _{k=1}^N{\overrightarrow{x}}_k$$

(2)

$$\Sigma =\frac{1}{N-1}\sum _{k=1}^N\left({\overrightarrow{x}}_k-\overrightarrow{\mu }\right){\left({\overrightarrow{x}}_k-\overrightarrow{\mu }\right)}^T$$

(3)

Here, ${\overrightarrow{x}}_k$ represents the three-dimensional coordinates of the point cloud in the voxel grid.

2) Initialize the transformation parameters to be solved, with zero values or odometry data. For each sample ${\overrightarrow{x}}_k$ in the S point cloud, transform it to the T point cloud according to the transformation parameters. Let ${\overrightarrow{x}}_k^{\prime }$ be the coordinate of ${\overrightarrow{x}}_k$ in the T point cloud coordinate system. Find the grid where ${\overrightarrow{x}}_k^{\prime }$ falls in the T point cloud, and combine the probability density function of each grid in the T point cloud to calculate the corresponding probability distribution function $p({\overrightarrow{x}}_k^{\prime })$.

$$p\left({\overrightarrow{x}}_k^{\prime }\right)\sim exp\left(-\frac{{\left({\overrightarrow{x}}_k^{\prime }-{\mu }_k\right)}^T{\sum }_k^{-1}\left({\overrightarrow{x}}_k^{\prime }-{\mu }_k\right)}{2}\right)$$

(4)

3) Add up the probability densities calculated for each mapped point to obtain the registration score.

$$score\left(p\right)=\sum _{k}exp\left(-\frac{{\left({\overrightarrow{x}}_k^{\prime }-{\mu }_k\right)}^T{\sum }_k^{-1}\left({\overrightarrow{x}}_k^{\prime }-{\mu }_k\right)}{2}\right)$$

(5)

Use the Newton optimization algorithm to optimize the objective function until the optimal transformation parameters are found that maximize the registration score, complete convergence, and solve the best rigid body transformation between the target and source point clouds to achieve accurate localization.

3. Inspection robot Path planning system

3.1. Sports model

Currently, the chassis of inspection robots mainly consists of legged, tracked, and wheeled types, each with its advantages and disadvantages in different environments. Legged inspection robots have strong terrain adaptability, but their structure and control system are complex. Tracked inspection robots have high traction and strong applicability in complex terrains such as outdoor, sandy, and muddy areas, but their speed is relatively low, and they have high motion noise. Wheeled inspection robots have fast speed, high efficiency, and low motion noise, and are widely used. In this paper, we focus on the complex indoor factory environment and adopt a two-wheel differential wheeled robot whose kinematic model is shown in Figure 4 [21].

In Figure 4, the motion of robot $R_i\left(i=1,2,\cdots ,n\right)$ is completed by two independently driven wheels. Let the radius of the driving wheel be $r_i$, and define the midpoint of the two driving wheels as $O_i$. The distance between the two wheels is $2b_i$, where $\{O,X,Y\}$ is the inertial Cartesian coordinate system, and $\{O_i,X,Y\}$ is the local coordinate system of the robot. $v_l$ is the speed of the left driving wheel, $v_{\mathrm{r}}$ is the speed of the right wheel, and $v_{\mathrm{c}}$ is the speed of the center of the robot. If $v_l\ne v_{\mathrm{r}}$, the angular velocity ${\omega }_i$ can be obtained. According to the robot model, the forward speed depends on the speed of the wheels.

$$v_{\mathrm{c}}=\frac{v_l+v_{\mathrm{r}}}{2}$$

(6)

The angular velocity $\omega$ is determined by the difference in speed between the left and right driving wheels and the distance between them.

$$\omega =\frac{v_l-v_{\mathrm{r}}}{2b_i}$$

(7)

In the ideal case, according to the principle of rigid motion, the trajectory of the robot is a circle, and the radius can be expressed as

$$R=\frac{v_{\mathrm{c}}}{\omega }=\frac{b_i\left(v_l+v_{\mathrm{r}}\right)}{\left(v_l-v_{\mathrm{r}}\right)}$$

(8)

The kinematic equation of the robot can be expressed as

$$q={\left[\begin{array}{ccc}x\hfill & y\hfill & \theta \hfill \end{array}\right]}^{\mathrm{T}}u$$

(9)

$$V={\left[\begin{array}{cc}{v}_c\hfill & \omega \hfill \end{array}\right]}^{\mathrm{T}}$$

(10)

$${\left[\begin{array}{ccc}\dot{x}\hfill & \dot{y}\hfill & \dot{\theta }\hfill \end{array}\right]}^{\mathrm{T}}=\left[\begin{array}{cc}cos\theta & 0\\ sin\theta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\frac{1}{2}& \frac{1}{2}\\ \frac{1}{b}& -\frac{1}{b}\end{array}\right]\left[\begin{array}{c}{v}_l\hfill \\ v_r\hfill \end{array}\right]$$

(11)

The above formula is the pose state matrix and motion state matrix, and the final formula can be written as follows:

$$\dot{q}=S\left(q\right)V=\left[\begin{array}{cc}cos\theta & 0\\ sin\theta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{v}_{\mathrm{c}}\hfill \\ \omega \hfill \end{array}\right]$$

(12)

where $S\left(q\right)$ is a $3\times 2$ smooth linearly independent matrix, and V is the motion matrix of the robot.

3.2. Path Planning

The path planning of inspection robots mainly relies on the constructed grid map to generate a safe and collision-free path by specifying the start and target locations. The path planning for robots can be divided into global path planning and local path planning.

(1) Global path planning

To ensure that the patrol robot can effectively avoid obstacles globally and locally, and considering that the grid map of the actual road scene is relatively simple, the A-star algorithm is used as the global path planning method to provide accurate obstacle avoidance directions for the robot through real-time planning [22,23]. A-star combines heuristic search with breadth-first algorithm to select the search direction through the cost function $f\left(n\right)$, and expands around the starting point. The cost value of each surrounding node is calculated by the heuristic function $h\left(n\right)$, and the minimum cost value is selected as the next expanding point. This process is repeated until the endpoint is reached, generating a path from the starting point to the endpoint. In the search process, since each node on the path is the node with the minimum cost, the cost of the path obtained is also minimum. The cost function of the A-star algorithm is

$$f\left(n\right)=g\left(n\right)+h\left(n\right)$$

(13)

where $f\left(n\right)$ is the cost function at the current position, $g\left(n\right)$ is the cost value from the starting position to the current position in the search space, and $h\left(n\right)$ is the cost value from the current position to the goal position. In the A-star algorithm, the selection of the heuristic function is crucial. Since the map environment is a grid map with obstacles, the Manhattan distance is used as the heuristic function, which is given by:

$$h\left(n\right)=|x_1-x_2|+|y_1-y_2|$$

(14)

where $(x_1,y_1)$,$(x_2,y_2)$ represent the coordinates from the current position to the target position. In the path planning of the A-star algorithm, the nodes are stored in two lists, Closelist and Openlist. The nodes that have been searched and generated cost values are stored in Openlist. The node with the minimum estimated cost is stored in Closelist, and the moving trajectory is formed by processing the trajectories of each node in Closelist. The specific steps are as follows:

Step 1.The starting point s of the robot is the first calculated point, and the surrounding nodes are added to Openlist, and the cost function $f\left(n\right)$ of each point is calculated.

Step 2.Search Openlist, select the node with the smallest cost value $f\left(n\right)$ as the current processing node n, remove the node from Openlist, and put it into Closelist.

Step 3.If the real cost value $g\left(n\right)$ of the adjacent node from the current processing node to the starting point s is smaller than the original $g\left(n\right)$ value, the parent node of the adjacent node is set to the current processing node; if it is larger, the current processing node is removed from Closelist, and the node with the second smallest value of $f\left(n\right)$ is selected as the current processing node.

Step 4.Repeat the above steps until the target point g is added to Closelist, traverse along each parent node, and the obtained node coordinates are the path.

(2)Local path planning

The working environment of inspection robots is not always static. In the process of moving along the global path, real-time obstacles may appear. To avoid collisions, the Timed-Elastic-Band (TEB) algorithm, which introduces local path planning with time elasticity, is used on the basis of global path planning to achieve real-time obstacle avoidance [24]. The TEB algorithm is an optimization algorithm that follows the path generated by the global path planner. The local trajectory it generates is composed of a series of continuous time and pose sequences, and the robot’s pose is defined as:

$$X_i={\left[x_i,y_i,{\beta }_i\right]}^T$$

(15)

where $X_i$ represents the i-th pose in the robot coordinate system, including position information $x_i$,$y_i$ and angle ${\beta }_i$. The time interval between adjacent poses $X_i$ and $X_{i+1}$ is denoted by $\Delta T_i$, as shown in Figure 5.

In the optimization process, the TEB algorithm applies graph optimization to the adjacent time intervals and states of the robot as nodes, and uses velocity, acceleration, and non-holonomic constraints of the robot as edges. It also considers obstacle information, discrete interval of planned trajectory, and adjacent temporal and spatial sequence constraints. Finally, the G2O solver is used to calculate the control variable $V(v,\omega )$ (where v and $\omega$ represent the linear and angular velocity of the robot, respectively), to obtain the optimal trajectory. The TEB algorithm obtains the optimal pose points through weighted multi-objective optimization [25,26], where the mathematical description of the objective function is:

$$f\left(B\right)=\sum _k{\gamma }_k{f}_k\left(B\right)$$

(16)

$$B^{*}=arg\underset{B}{min}f\left(B\right)$$

(17)

where $f\left(B\right)$ is the objective function that considers various constraints, $f_k\left(B\right)$ is the constraint function, ${\gamma }_k$ is the weight of each item, and $B^{*}$ is the optimal TEB trajectory. The TEB algorithm has four constraint functions.

1) Path following and obstacle constraint objective function

The TEB algorithm aims to avoid collisions with static or dynamic obstacles while following the path. The algorithm treats piecewise continuous and differentiable functions as constraints and punishes behaviors that do not conform to the constraints. Specifically:

$$e_{\Gamma }\left(x,x_r,\epsilon ,S,n\right)\approx \left\{\begin{array}{cc}{\left(\left(x-\left(x_r-\epsilon \right)\right)/S\right)}^n\hfill & x>x_r-\epsilon \hfill \\ 0\hfill & \mathrm{other}\hfill \end{array}\right.$$

(18)

$$f_{\mathrm{path}}=e_{\Gamma }\left(d_{min,j},r_{p_{max}},\epsilon ,S,n\right)$$

(19)

$$f_{\mathrm{ob}}=e_{\Gamma }\left(-d_{min,j},-r_{p_{min}},\epsilon ,S,n\right)$$

(20)

Building on Equation (18), penalty functions $f_{\mathrm{path}}$ and $f_{\mathrm{ob}}$ are constructed. Here, $x_r$ denotes the boundary, $\epsilon$ is the offset factor, S is the scaling factor, n is the order, $d_{min,j}$ is the independent variable representing the distance between the path point and obstacle, $r_{P_{max}}$ is the maximum distance of the trajectory deviation from the path point, and $r_{o_{min}}$ is the minimum distance between the trajectory and obstacle.

2) The velocity and acceleration constraint functions of a robot

According to the dynamic equation, the constraint functions of the robot’s velocity and acceleration are expressed as equations (21) - (24) :

Linear velocity:

$$v_i\approx \frac{1}{\Delta T_i}\left(\begin{array}{c}{x}_{i+1}-x_i\hfill \\ y_{i+1}-y_i\hfill \end{array}\right)$$

(21)

Angular velocity:

$$w_i\approx \frac{{\theta }_{i+1}-{\theta }_i}{\Delta T_i}$$

(22)

Linear acceleration:

$$a_i\approx \frac{2\left(v_{i+1}-v_i\right)}{\Delta T_i+\Delta T_{i+1}}$$

(23)

Angular acceleration:

$${\alpha }_i=\frac{2\left(w_{i+1}-w_i\right)}{\left(\Delta T_i+\Delta T_{i+1}\right)}$$

(24)

3) Non-holonomic constraint:

The robot used in the algorithm simulation and experiment is a differential drive structure with two degrees of freedom, which cannot perform translational motion along the y-axis of the robot coordinate system. The curvature of the circular arc between two adjacent robot poses is approximately constant, and the outer product of the direction vector $d_{i,i+1}$ and the turning angle ${\theta }_i$ between adjacent poses in the robot coordinate system is equal to the outer product of the turning angle ${\theta }_{i+1}$ and the direction vector $d_{i,i+1}$. ${\beta }_i$ represents the orientation of the robot in the global coordinate system, and the corresponding relationship equation and non-holonomic constraint are:

$$\left(\begin{array}{c}cos{\beta }_i\\ sin{\beta }_i\\ 0\end{array}\right)\times d_{i,i+1}=d_{i,i+1}\times \left(\begin{array}{c}cos{\beta }_{i+1}\\ sin{\beta }_{i+1}\\ 0\end{array}\right)\iff {\theta }_i={\theta }_{i+1}$$

(25)

$$d_{i+1}:=\left(\begin{array}{c}{x}_{i+1}-x_i\\ y_{i+1}-y_i\\ 0\end{array}\right)$$

(26)

$$f_k\left(X_i,X_{i+1}\right)={∥\left[\left(\begin{array}{c}cos{\beta }_i\\ sin{\beta }_i\\ 0\end{array}\right)+\left(\begin{array}{c}cos{\beta }_{i+1}\\ sin{\beta }_{i+1}\\ 0\end{array}\right)\right]\times d_{i,i+1}∥}^2$$

(27)

The objective function $f_k(X_i,X_{i+1})$ punishes the quadratic error for violating this constraint, ensuring that the output velocity of the robot follows the non-holonomic constraint.

4) Fastest path constraint

The TEB algorithm incorporates the time interval information between poses, and the total time is the sum of all time intervals. The relevant objective function is:

$$f_k={\left(\sum _{i+1}^n\Delta T_i\right)}^2$$

(28)

After optimizing the TEB sequence, the objective function of the constraints is optimized to ensure that the path planned by the algorithm achieves the best results in terms of obstacle avoidance, time, and distance.

(3)Path planning based on fusion algorithm

The A-star algorithm yields a navigation path consisting of only the start point, key points, and destination point, but it cannot avoid unknown obstacles in the environment. The TEB algorithm exhibits good local obstacle avoidance ability, but with only a single final goal point as a guide, it is prone to becoming trapped in local optima. Therefore, we propose a hybrid path planning algorithm that combines the strengths of both algorithms. The specific algorithm process is shown in Figure 6.

Global path planning takes a static obstacle cost map as input, and does not consider the robot’s mechanical performance and kinematic constraints when planning the path. It uses the A-star algorithm to plan the optimal path from the robot’s current position to the desired target position, and provides an initial value for local planning.

Local path planning collects path nodes on the global optimal path, and optimizes the global path subset between the robot’s current node and the collected path nodes. It combines the static obstacle cost map and dynamic obstacle cost map, and uses the TEB algorithm to continuously adjust the pose and orientation of the robot during its movement, taking into account its shape, dynamic model, and motion performance in the scope of local planning. When encountering dynamic obstacles, it removes the old robot pose and adds a new robot pose, so that a new path can be generated in each iteration, and an optimized path can be obtained through continuous iteration.

By fusing navigation algorithms, we achieve optimal global path planning and real-time obstacle avoidance functionality in the process of mobile robot navigation.

4. Experiment and Analysis

4.1. Experiment settings

In order to verify the effectiveness of navigation systems in practical applications, we conducted experiments using a Turtlebot3 mobile robot in different types of scenarios built using the Gazebo simulation platform within the ROS on a 64-bit Ubuntu 18.04 operating system with 4GB of running memory. As shown in Figure 7, we constructed a home environment and a library environment to simulate real-world environments. Using the real-time localization and mapping capabilities in Rviz, we scanned the simulated environments, constructed corresponding maps, and performed path planning.

4.2. Performance evaluation

4.2.1. Visual-SLAM Algorithm performance evaluation

This paper presents a method for constructing a corresponding point cloud map using a depth camera in a ROS environment. The depth camera data is first read in the ROS environment, and then the front-end and back-end threads are executed to construct a sparse feature point map, which is continuously updated to create a real-time point cloud map. Keyframes from the front-end are passed into the point cloud construction thread to generate the point cloud map. The effectiveness of the proposed algorithm for generating maps is validated by the corresponding point cloud map in Figure 8, which demonstrates good 3D effects for constructing maps in indoor environments. As shown in Figure 8, the algorithm detects the object’s motion trajectory, which is consistent with the actual trajectory. Although there are deviations between the detected trajectory and the actual trajectory, there is no serious deviation, which satisfies the perception requirements of the robot. When the object’s motion trajectory changes significantly, there is still no serious deviation, which also meets the perception requirements of the robot.

4.2.2. Multi-line LiDAR-based SLAM Algorithm performance evaluation

From the comparison between point cloud mapping and visual mapping, it can be observed that maps constructed using multi-line laser scanning are clearer than those constructed using visual algorithms, which reduces accumulated errors and provides better handling of edge contours. Furthermore, a comparison was conducted on mapping time, mapping effectiveness, and CPU utilization, in order to validate the feasibility, reliability, and accuracy of the algorithm.

To ensure the accuracy of the experiments, multiple tests were conducted. The robot was fixed at a certain position, denoted as the origin (0,0), and the output object motion data was compared with the actual object motion data. The results are shown in Figure 9. The algorithm detected that the point cloud map was generally consistent with the simulated scene, and that the detected trajectory was not significantly deviated from the actual trajectory, which satisfies the perception requirements of the robot. As shown in the figure, when there were large changes in the object’s motion trajectory, there was a slight deviation between the detected trajectory and the actual trajectory, but no serious deviation occurred, which still satisfied the perception requirements of the robot.

4.2.3. Path Planning Performance Evaluation

Through testing, the path distance planned by the A-star algorithm has a certain distance from the obstacles, which avoids the collision of the robot. At the same time, global path planning has a good effect and can accurately reach the set target point location, satisfying the requirement of precise navigation. The robot moves along a square path. When encountering obstacles, it autonomously avoids them through local path planning. The process and result of local path planning are shown in Figure 10. After configuring the relevant parameters, observe the 3D view area of robot navigation in Rviz. The environment of the map is displayed as a global cost map, and the environment around the robot is a local cost map. The blue area is the expansion layer of the obstacle, which is expanded outward on the map to avoid collision between the robot and the obstacle. By adding the Path plugin in RViz, you can see the path that the robot moves. The green line is the route of global path planning, and the red line is the route of local path planning. It can be seen from Figure 10 that the local path planning route of the inspection robot is smooth and the planned route does not enter the expansion layer of the obstacle, which can reasonably avoid the surrounding obstacles and has a good obstacle avoidance effect. The global path planning is shown in Figure 11. After testing, the inspection robot can accurately achieve autonomous obstacle avoidance and complete local path planning for the set target point, satisfying the requirement of precise navigation.

5. Conclusions

The utilization of intelligent inspection robots has been shown to enhance production efficiency and reduce costs. However, the complex factory environment, filled with machinery equipment, pipelines, cables, and other obstacles, can pose a challenge to accurate inspections. To address this, we have developed a high-precision navigation inspection system that is specifically designed for complex factory scenes. The system is equipped with two types of sensors, visual and LiDAR, to allow for rich environmental information and localization and mapping. Optimal path planning is achieved by combining the A-star algorithm and TEB algorithm for dynamic programming. To evaluate the performance of the navigation system, simulations were conducted in two scenarios using Gazebo simulation software in the ROS system: a residential area and a library. Results indicate that the navigation system provides real-time localization and map construction, can navigate mobile platforms, and implements real-time obstacle avoidance in different scenarios. As such, this technology can be applied to the localization and navigation system of wheeled inspection robots in various complex environments, and has significant reference value.