3.1.1. Joint calibration

The data obtained by 2D LiDAR and thermal imager are based on their respective coordinate systems. In order to realize the data fusion of 2D LiDAR and thermal imager, it is necessary to unify their data into the same coordinate system, and the key is to determine the rigid coordinate transformation relationship between the two sensors.

Suppose one point P is $\boldsymbol{P}_{l}=(X_{l},Y_{l},Z_{l})^{T}$ in the 2D LiDAR coordinate system and is $\boldsymbol{P}_{t}=(X_{t},Y_{t},Z_{t})^{T}$ in the thermal imager coordinate system. The relation between P _l and P _t can be expressed as

(1) \begin{equation} \boldsymbol{P}_{\boldsymbol{c}}=\left(\begin{array}{l} X_{c}\\ Y_{c}\\ Z_{c} \end{array}\right)=\boldsymbol{R}\boldsymbol{P}_{\boldsymbol{l}}+\boldsymbol{t}=\boldsymbol{R}\left(\begin{array}{l} X_{l}\\ Y_{l}\\ Z_{l} \end{array}\right)+t. \end{equation}

where R and t are the rotation matrix and the offset matrix of the LiDAR coordinate system and the thermal imager coordinate system. The coordinate information of the camera space point can be represented by the pixel coordinate of the projection point $(u,v)$ and the depth value Z _c of the corresponding projection point. The conversion relationship between P _l and camera pixel coordinates can be expressed as

(2) \begin{equation} \left(\begin{array}{l} u\\ v\\ 1 \end{array}\right)=\frac{1}{Z_{c}}\left(\begin{array}{l@{\quad}l@{\quad}l} f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ 0 & 0 & 0 \end{array}\right)\left(\begin{array}{l} X_{c}\\ Y_{c}\\ Z_{c} \end{array}\right)=\frac{1}{Z_{c}}\boldsymbol{K}\left(\begin{array}{l} X_{c}\\ Y_{c}\\ Z_{c} \end{array}\right)=\frac{1}{Z_{c}}\boldsymbol{K}\left(\boldsymbol{R}\left(\begin{array}{l} X_{l}\\ Y_{l}\\ Z_{l} \end{array}\right)+\boldsymbol{t}\right). \end{equation}

where $f_{x}$ and $f_{y}$ are the focal length of the camera, $c_{x}$ and $c_{y}$ are the offset of the pixel coordinate system, K is the internal parameter matrix of the camera. The goal of joint calibration is to determine K , R , and t .

3.1.2. Coordinate transformation

After coordinate transformation, LiDAR and thermal imager are in the same coordinate system. In order to fuse the camera point cloud and LiDAR point cloud, the radar point cloud needs to be projected onto the plane where the thermal imager map is located, as shown in Figure 2.

Figure 2.

Camera 3D point cloud projection.

Figure 2 Long description

The diagram illustrates the projection of a 3D point cloud onto a thermal image plane. It features a coordinate system with axes labeled X, Y, and Z, and a thermal image plane intersecting these axes. Points in the 3D space, labeled as P(Xt, Yt, Zt), are projected onto the thermal image plane at positions labeled P(u, v). The diagram includes labels for the origin Ot, the thermal image plane, and the coordinates x, y, and o on the image plane. Dashed lines represent the projection of 3D points onto the 2D thermal image plane.

The coordinates of the points projected by the LiDAR point cloud onto the plane of the thermal imager map are expressed as

(3) \begin{equation} \left\{\begin{array}{l} u=\dfrac{f_{x}\ast X_{c}}{Z_{c}}+c_{x}\\[8pt] v=\dfrac{f_{y}\ast Y_{c}}{Z_{c}}+c_{y} \end{array}\right. .\end{equation}

The mismatch between the higher resolution of the thermal imaging image and the point cloud formed by the LiDAR is projected onto the image. This results in the LiDAR point cloud after projection being relatively sparse on the thermal imaging image. To extract temperature values from the thermal imaging image, we need to associate each LiDAR point with a pixel in the thermal imaging image. For each projection point of a single scan, the neighborhood range of the point is expanded to obtain more temperature information. After removing points that are beyond the image boundaries or have invalid depth, for each projection point of a single scan, the neighborhood range is expanded to obtain more temperature information. A circular neighborhood is extracted centered on the projection point (u, v), and the Euclidean distance of all pixels within the neighborhood to (u, v) is calculated.

(4) \begin{equation} d_{k}=\sqrt{\left(i_{k}-u\right)^{2}+\left(j_{k}-v\right)^{2}} .\end{equation}

The temperature value t _k is extracted from the selected pixel set, and the average value t is calculated.

(5) \begin{equation} \left\{\begin{array}{l} t=\dfrac{\sum _{k=1}^{n}w_{k}\ast t_{k}}{\sum _{k=1}^{n}w_{k}}\\[8pt] w_{k}=exp\!\left({-}d_{k}^{2}/\sigma ^{2}\right) \end{array}\right. .\end{equation}

The temperature value t of the projection point is obtained through Gaussian weighted averaging. The weight w _k is an exponential decay function of the distance from the pixel to the center point. The closer the distance, the greater the weight. The parameter σ controls the speed of the decay.

The temperature value t of these hot spots is added to the original coordinates of the point cloud projection point, denoted as $\boldsymbol{P}_{tl}=(X_{l},Y_{l},Z_{l},T)$ .

The result after superposition fusion is shown in Figure 3. The laser point cloud data and pixel images of the thermal imager are obtained in the same coordinate system by conversion. The data from the two sensors are superimposed and used for building temperature-enhanced point clouds.

Figure 3.

Temperature-enhanced point cloud after sensor fusion.

Due to the different data collection frequencies of LiDAR and thermal imagers, time synchronization is required before fusion. This can be achieved through timestamp alignment. Timestamps are recorded for each LiDAR scan and thermal image frame. For each LiDAR scan frame, the thermal image frame with the closest timestamp is found.

(6) \begin{equation} T_{\textrm{sync}}=arg\min _{{\unicode[Arial]{x0394}} t}\parallel P_{\text{lidar}}(t)-P_{\text{thermal}}\left(t+{\unicode[Arial]{x0394}} t\right)\parallel \end{equation}

When T _sync is less than the set value, data fusion is performed. If no synchronized frame is found, skip this LiDAR scan.

3.1.3. Loop closure detection in hot environments

The temperature-enhanced point cloud obtained by fusing thermal images and LiDAR sensors is used for closed-loop detection, which is applied to enhance the robustness, accuracy, and scene adaptability of the SLAM system, especially in the high-temperature environment described in this paper. During the robot’s movement, inevitable mapping accumulation errors will occur. The closed-loop detection identifies loops to provide a strong constraint for the system, pulling the current pose and historical poses together, thereby significantly correcting the global consistency of the entire trajectory and map.

In this part, we propose to use geometric-thermal joint features for the closed-loop detection part. Features of 2D LiDAR point clouds and thermal imaging data are extracted to construct geometric-thermal joint feature descriptors.

In 2D point clouds, we usually calculate geometric features using points and their neighborhoods. Here, we adopt an algorithm similar to Fast Point Feature Histogram (FPFH), but simplify it to 2D. For point p and one of its neighboring points q _n , the following characteristics are calculated.

• • The angle $\alpha$ between the vector pq _n and the tangent vector at point p is obtained through local linear fitting within the domain.

• • The angle $\beta$ between the tangent vector at point q _n and vector pq _n .

• • The angle θ between the normal vectors at point p and point q _n .

Then, the angle features ( $\alpha$ , $\beta$ , θ) between point p and each of its neighboring points are statistically analyzed to form a histogram. Each angle is divided into several intervals, and the frequency of falling into each interval is counted. Thus, each point has a geometric feature histogram. This histogram is the simplified FPFH feature of the point, which we denote as F _geom( p ).

For each point p , not only does it have its geometric information, but thermal imaging data is fused to attach a temperature value t to each point. Therefore, the following thermal features of point p and its domain point q _n are extracted.

1. – The average temperature of the neighborhood is as follows.

   (7) \begin{equation} \mu _{t}=\left(1/k\right)\ast \sum t_{i} \end{equation}

   i is the index of the neighborhood points, including point p itself.

2. – The standard deviation of the neighborhood temperature is as follows.

   (8) \begin{equation} \sigma _{t}=\sqrt{\left(1/k\right)* \sum \left(t_{i}- \mu _{t}\right)^{2}} \end{equation}

3. – Divide the temperature range into several intervals, count the number of points in each temperature interval within the neighborhood, and form a neighborhood temperature distribution histogram, denoted as hist _t .

These three parts combine to form a thermal feature vector F _thermal( p ) = [μ _t , σ _t , hist _t ].

The geometric feature histogram F _geom( p ) and the histogram hist _t in the thermal feature are also normalized. μ _t and σ _t are standardized using the temperature mean and standard deviation of the entire scene.

(9) \begin{equation} \left\{\begin{array}{l} \mu _{\text{global}}=\left(1/N\right)*\sum t_{i} \\ \sigma _{\text{global}}=\sqrt{\left(1/N\right)* \sum (t_{i}- \mu _{\text{global}})^{2}}\\ \mu '_{t}=(\mu _{t}-\mu _{\text{global}})/\sigma _{\text{global}}\\ \sigma '_{t}=\sigma _{t}/\sigma _{\text{global}} \end{array}\right. \end{equation}

μ _global is the global average temperature, and σ _global is the global temperature standard deviation.

The normalized geometric features and thermal features are combined into a descriptor F( p ).

(10) \begin{equation} F \! \left(p\right)=\left[F'_{\text{geom}}\left(p\right),F'_{\text{thermal}}\left(p\right) \right] \end{equation}

Suppose that the key frame k contains N points. The global descriptor obtained by averaging the descriptors of the N points in the key frame can be expressed as follows.

(11) \begin{equation} G(k)=1/N*\sum _{i=1}^{N}F(p_{i}) \end{equation}

For key frames k and c, calculate their global descriptors, respectively. Split the global descriptors into geometric and thermal parts as follows.

(12) \begin{equation} \left\{\begin{array}{l} G(k)=\left[G_{\text{geom}}(k),G_{\text{thermal}}(k)\right]\\[5pt] G(c)=\left[G_{\text{geom}}(c),G_{\text{thermal}}(c)\right] \end{array}\right. \end{equation}

Then, the geometric and thermal Euclidean distances are calculated separately as follows.

(13) \begin{equation} \left\{\begin{array}{l} d_{g}=\left\| G_{\text{geom}}(k)-G_{\text{geom}}(c)\right\| _{2}\\[6pt] d_{t}=\left\| G_{\text{thermal}}(k)-G_{\text{thermal}}(c)\right\| _{2} \end{array}\right. \end{equation}

Set the geometric distance threshold ${\tau}_{g}$ and the thermal distance threshold ${\tau}_{t}$ . When the following conditions are simultaneously satisfied ${{d}_{g}} \lt {\tau}_{g}$ and ${{d}_{t}} \lt {\tau}_{t}$ , then it is considered that the two key frames have passed the geometric-thermal consistency verification, that is, they are matched.

This method decomposes the global descriptors to independently verify the geometric and thermodynamic consistency, thereby avoiding incorrect matches in the dynamic thermal environment. Closed-loop detection of the fused thermal image and LiDAR can construct a more accurate and robust, globally consistent map.

3.2.1. Temperature-enhanced point cloud pretreatment

Due to the influence of hardware characteristics and detection noise, the temperature-enhanced point cloud obtained by LiDAR in thermal environment has the following problems: (1) The density of point cloud attenuates with the increase of detection distance, resulting in the overfitting of the temperature field at the near end and the loss of the features at the far end, which directly affects the accuracy of the temperature repulsion force; (2) Abnormal temperature value is introduced by sensor noise and environmental thermal disturbance, which is easy to cause path oscillation when directly used in repulsive field calculation; (3) The computational complexity of potential fields involving high-density discrete point clouds is high, which affects the computational efficiency of mobile robots. In order to alleviate the negative impact of the above problems on path generation, this paper adopts the grid method to preprocess the original point cloud.

The continuous two-dimensional space is divided into grids by fixed side lengths, each grid is identified by its index (i, j), and the central coordinate of each grid is calculated as the representative position. Each grid may contain multiple obstacle points, and the mean temperature of all points within the grid needs to be aggregated into a representative value.

(14) \begin{equation} T_{\text{grid}}=\frac{1}{N}\sum _{k=1}^{N}T_{ob,N} \end{equation}

If there are point clouds within the grid, each grid will be regarded as a circular obstacle. Its influence radius covers the entire grid area, ensuring that a path can be found by avoiding the grid edges.

(15) \begin{equation} R_{{\rm grid}}=\frac{\sqrt{\left({\unicode[Arial]{x0394}} x\right)^{2}+\left({\unicode[Arial]{x0394}} y\right)^{2}}}{2}=\frac{\sqrt{2}}{2}*L_{{\rm grid}} \end{equation}

where Δx = Δy = L _grid is the side length of the grid.

Due to the closed or obstructive environment, the intensity of the heat radiated by the high-temperature grid points to the surrounding space follows the exponential attenuation model in this paper. For a single high-temperature grid heat source HS(x _s , y _s, T _s ), the temperature value at which its thermal radiation decays to any grid (x, y) is

(16) \begin{equation} T\left(x,y\right)=\left\{\begin{array}{l@{\quad}l} T_{s}\cdot e^{-\lambda \cdot d\left(x,y\right)}&{\rm if}\ d\left(x,y\right)\leq r\\ 0&\text{otherwise} \end{array}\right. \end{equation}

Among them, $\unicode{x03BB}$ is the speed of temperature diffusion controlled by the attenuation coefficient, and $d(x,y)=\sqrt{(x-x_{s})^{2}+(y-y_{s})^{2}}$ is the distance from the target point to the heat source point. r is the effective radiation radius of the heat source. Beyond this range, it is considered to have no effect. If the target point is affected by multiple high-temperature grids, its temperature values are superimposed according to the radiation intensity.

This method can reduce the uncertainty of environmental modeling and provide a feasible framework for temperature sensing path planning, and its practical effect will be verified in subsequent experiments.

3.2.2. Artificial potential field

The artificial potential field method is a common method for local path planning. It is the movement of the robot in the surrounding environment, designed as an abstract artificial gravitational field movement. The target point produces “gravity” in the mobile robot, and the obstacle produces “repulsion” in the mobile robot. Finally, the motion of the mobile robot is controlled by finding the resultant force.

The artificial potential field method models the environment as a potential field. The target point is attractive to the robot, and the potential field function can be expressed as

(17) \begin{equation} U_{{\rm att}}(q)=\frac{1}{2}k_{att}\cdot d\left(q,q_{{\rm goal}}\right)^{2} .\end{equation}

where q is the current position of the robot, q _goal is the target point, and k _att is the attraction coefficient.

Obstacles generate a repulsive force on the robot, and the potential field function can be expressed as

(18) \begin{align} U_{{\rm rep}}(q)=\left\{\begin{array}{l@{\quad}l} \dfrac{1}{2}k_{{\rm rep}}\cdot \left(\dfrac{1}{d\left(q,q_{{\rm obs}}\right)}-\dfrac{1}{d_{0}}\right)^{2}d^{2}\left(q,q_{{\rm goal}}\right)&\text{ if }d\left(q,q_{\text{obs}}\right)\leq d_{0} \\[10pt] 0&\text{ otherwise} \end{array}\right. . \end{align}

where q _obs is the position of the obstacle, d ₀ is the influence range of the obstacle, and k _rep is the repulsive force coefficient. The repulsive force of the obstacle does not always exist, but only occurs when the distance between the obstacle and the robot is less than the distance threshold.

3.2.3. Path planning with thermal information

By integrating the thermal information into the potential field method, the high-temperature area is regarded as an obstacle to increasing the repulsive force, and the potential field strength is dynamically adjusted according to the gradient distribution of the thermal value.

The point cloud data is converted into a raster map, and the thermal value T is assigned to each raster. The repulsive potential field in the high-temperature region can be expressed as

(19) \begin{equation} U_{\text{heat-rep}}(q)=\left\{\begin{array}{l@{\quad}l} \dfrac{1}{2}k_{\text{heat-rep}}\cdot \dfrac{T\left(q_{\text{heat}}\right)-T_{\text{thre}}}{\max \left(d\left(q,q_{\text{heat}}\right)^{2},d_{\text{heat-}\min }^{2}\right)}&\text{ if }T\left(q_{\text{heat}}\right)\geq T_{\text{thre}}\\ 0&\text{ otherwise} \end{array}\right. . \end{equation}

where k _heat-rep is the coefficient of thermal repulsion force, T(q) is the thermal value of the high-temperature region, and T _thre is the threshold of the high-temperature region. k _heat-rep will adjust according to the range and importance of the thermal value and need to coordinate with the repulsive force of the obstacle to avoid conflict. The repulsive force weight is reduced at high temperatures near the target point, as follows.

(20) \begin{equation} k_{\text{heat-rep}}=k_{\text{heat-rep-base}}\left(1-e^{-\frac{d\left(q,q_{\text{goal}}\right)^{\beta }}{2}}\right). \end{equation}

$\beta$ is the attenuation coefficient that controls how fast the repulsive force decays as it approaches the target. The temperature-repulsive force of the obstacle is generated only when the distance between the obstacle and the robot is less than the threshold of temperature action. Based on the empirical formula of temperature decay with distance

(21) \begin{equation} T_{d}=T_{0}*e^{-d/\alpha } \end{equation}

where T ₀ is the temperature of the heat source and T _d is the temperature at a distance d from the heat source. By substituting T _thre into T _d , the distance between the temperature threshold and the heat source can be expressed as

(22) \begin{equation} D_{\text{thre}}=\alpha *\left(\ln T_{0}-\ln T_{\text{thre}}\right) \end{equation}

The temperature-repulsive force of the obstacle only acts when the distance from the heat source is less than D _thre.

After superimposing all potential fields, the total potential field function can be expressed as

(23) \begin{equation} U_{\text{total}}(q)=U_{\text{att}}(q)+U_{\text{rep}}(q)+U_{\text{heat-rep}}(q) \end{equation}

Gradient descent is used to find a path from the starting point to the target point in the total potential field.

(24) \begin{equation} q_{\text{next}}=q_{\text{current}}-\nabla \ast U_{\text{total}} (q_{\text{current}}) \end{equation}

The potential field coefficients k _rep and k _heat-rep are adjusted to achieve a balance between obstacle avoidance and thermal constraints. Through the above method, the point cloud map based on thermal information and the artificial potential field method can be realized.

In the process of path generation based on the potential field method, although the introduction of the discretization step guarantees the convergence of the algorithm, it inevitably leads to the non-smooth oscillation of the path. This paper uses the Savitzky–Golay filter for path smoothing, which is a smoothing algorithm based on local polynomial fitting. The core idea is to preserve the characteristics of the signal by polynomial fitting in the sliding window while suppressing noise-induced fluctuations. The SG filter maintains the geometry of the signal while smoothing.

For the path sequence Q = {q ₁, q ₂, …, q _n }, the polynomial is fitted in the sliding window, and the formula is as follows.

(25) \begin{equation} \begin{cases} x(t)=a_{0}+a_{1}t+a_{2}t^{2}+\cdots +a_{k}\hspace{0pt}t^{k}\\ y(t)=b_{0}+b_{1}t+b_{2}t^{2}+\cdots +b_{k}\hspace{0pt}t^{k} \end{cases} \end{equation}

where t is the local time coordinate in the window and k is the polynomial order. The coefficients {a _k }, {b _k } are solved by the least square method, and the value of the fitting polynomial at the center point of the window is used as the smoothed output. The filtered coordinates can be expressed as

(26) \begin{equation} Q_{\text{smooth}}=SG\_ \text{Filter} (Q,k,w) \end{equation}

where k is the polynomial order, representing the highest degree of fitting the polynomial and w is the window size representing the sum of the current points used in each fit and the number of points before and after them. The smoothed path after fitting is shown in Figure 4.

Figure 4.

The original path and smooth path are represented by the red dotted line and the blue solid line, respectively.

Figure 4 Long description

A scatter plot displays a collection of data points with a red dotted line and a blue solid line representing different paths. The red dotted line indicates the original path, while the blue solid line represents the smooth path. The plot includes a legend in the top right corner, identifying the start and end points with blue and green dots, respectively. The x-axis and y-axis represent numerical values, with the data points scattered across the plot. The plot shows clusters of points and distinct paths, illustrating the comparison between the original and smooth paths. All values are approximated.

The pseudocode of the path-planning algorithm with thermal map is shown below.

Algorithm 1.

Temperature-enhanced Artificial Potential Field Path Planning.

Algorithm 1 Long description

The pseudocode outlines a path-planning algorithm that incorporates thermal maps. It starts with initializing the path and current position. The algorithm iterates to update the path by calculating attractive and repulsive forces from obstacles and thermal influences. It checks for convergence and updates the current position accordingly. The process repeats until the maximum number of iterations is reached or the goal is achieved.