阅读说明：本技术 一种基于灯光标定信息融合的激光slam方法及装置 (Laser SLAM method and device based on lamplight calibration information fusion ) 是由 王强 曾勇 高川琦 卢镇宇 于 2020-07-09 设计创作，主要内容包括：本发明公开一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质,属于激光SLAM与Li-Fi信息传输技术领域,解决了现有技术中位姿漂移过大及建立的地图准确性较低的技术问题。一种基于灯光标定信息融合的激光SLAM方法,包括以下步骤：获取最优链路链接,根据所述最优链路链接,确定基于Li-Fi的帧间齐次变换矩阵；获取数据结合帧数据,对所述数据结合数据进行配准,得到配准后的数据结合帧数据,确定基于激光的帧间齐次变换矩阵,获取融合Li-Fi和激光数据帧之间的位姿变换矩阵,根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据,对当前帧与地图进行匹配并更新地图。抑制了位姿漂移,提高了建立的地图的准确性。(The invention discloses a laser SLAM method and device based on lamplight calibration information fusion and a computer readable storage medium, belongs to the technical field of laser SLAM and Li-Fi information transmission, and solves the technical problems of overlarge pose drift and lower accuracy of an established map in the prior art. A laser SLAM method based on lamplight calibration information fusion comprises the following steps: obtaining an optimal link, and determining a homogeneous transformation matrix between frames based on Li-Fi according to the optimal link; acquiring data combination frame data, registering the data combination data to obtain the registered data combination frame data, determining a laser-based interframe homogeneous transformation matrix, acquiring a pose transformation matrix between a fusion Li-Fi and a laser data frame, and matching a current frame with a map and updating the map according to the pose transformation matrix between the fusion Li-Fi and the laser data frame and the registered data combination frame data. The pose drift is inhibited, and the accuracy of the established map is improved.)

1. A laser SLAM method based on lamplight calibration information fusion is characterized by comprising the following steps:

acquiring a calibration coordinate of an LED, a code of an LED transmitter and an estimated coordinate of a receiver, and acquiring an optimal link according to the calibration coordinate of the LED, the code of the LED transmitter and the estimated coordinate of the receiver;

obtaining a current data frame based on Li-Fi according to the optimal link, and determining a homogeneous transformation matrix between frames based on Li-Fi according to the current data frame based on Li-Fi;

acquiring current laser frame data, acquiring data combination frame data according to the current laser frame data and an interframe homogeneous transformation matrix based on Li-Fi, registering the data combination data to obtain registered data combination frame data, and determining an interframe homogeneous transformation matrix based on laser according to the registered combination frame data;

and matching the current frame with a map and updating the map according to the pose transformation matrix between the Li-Fi and the laser data frame and the registered data combination frame data.

2. The laser SLAM method based on lamplight calibration information fusion of claim 1, wherein obtaining optimal link links according to the calibration coordinates of the LEDs, the codes of the LED transmitters and the estimated coordinates of the receivers comprises establishing an optimal link mechanism model, and optimally linking the links through the optimal link mechanism model, wherein the optimal link mechanism model is

T_xi＝(x_i,y_i,i)，(x_i,y_j) For the calibration coordinates of the LED, i-1, 2,3 are the LED transmitter codes, R_xt＝(x_e,y_eT), t is the observation time, (x)_e,y_e) To estimate the coordinates of the receiver, E ═ 1,1,0)^T，Link_nIs the number of Li-Fi links.

3. The laser SLAM method based on light calibration information fusion of claim 1, wherein the obtaining of data combination data according to current laser frame data and an interframe homogeneous transformation matrix based on Li-Fi specifically comprises obtaining data combination data according to the Li-Fi homogeneous transformation matrix T₁The method comprises the steps of carrying out mapping transformation on current laser frame data to obtain virtual frame data, carrying out spatial adjacent sequential preprocessing on the virtual frame data to obtain preprocessed virtual frame data, transforming the current laser frame data to a reference frame coordinate system to obtain reference frame data, and adding the reference frame data into the virtual frame data to obtain data combination frame data.

4. The laser SLAM method based on light calibration information fusion of claim 3, wherein transforming the current laser frame data to the reference frame coordinate system to obtain the reference frame data comprises transforming the current laser frame data to the reference frame coordinate system by inter-frame homogeneous transformation, wherein the change relationship of the inter-frame homogeneous transformation is

Wherein, T_j＝(x_j,y_j,θ_j)∈R²×[0,2π]Is a pose transformation matrix, when j is 1, T_jFor a Li-Fi-based inter-frame homogeneous transformation matrix, when j is 2, T_jFor a laser-based inter-frame homogeneous transformation matrix,represents a data point q_lBy transforming the matrix T_jConversion to q_l,f，f(T_j,q_l) For the corresponding mapping transformation function, q_l＝(q_lx,q_ly)^TAre the observed coordinates.

5. The laser SLAM method based on light calibration information fusion of claim 4, wherein determining the Li-Fi based inter-frame homogeneous transformation matrix and the laser based inter-frame homogeneous transformation matrix specifically comprises determining the Li-Fi based inter-frame homogeneous transformation matrix T by a relative transformation solving formula₁Pose transformation matrix T corresponding to laser-based interframe homogeneous transformation₂The relative transformation solving formula is

Wherein the content of the first and second substances,pose for the kth iterationThe matrix is transformed by a transformation matrix which is,representing data points_qlTransformed matrixTransformation to data point q_l,f，

6. The laser SLAM method based on lamplight calibration information fusion of claim 1, wherein the registration of the data combination data specifically comprises calculating Euclidean distances between current data and adjacent data, and using two data with the smallest Euclidean distances as registration data to realize the registration of the data combination data.

7. The laser SLAM method based on light calibration information fusion of claim 1, wherein a pose transformation matrix between the fused Li-Fi and the laser data frame is obtained according to the Li-Fi based interframe homogeneous transformation matrix and the laser based interframe homogeneous transformation matrix, and specifically comprises obtaining the pose transformation matrix between the fused Li-Fi and the laser data frame according to the Li-Fi based interframe homogeneous transformation matrix, the laser based interframe homogeneous transformation matrix and the probability-based weight coefficient.

8. The laser SLAM method based on light calibration information fusion of claim 1, further comprising obtaining observation probability based on Li-Fi data according to an observation probability model based on Li-Fi data, obtaining observation probability based on laser data according to an observation probability model based on laser data, and obtaining weight coefficient based on probability according to the observation probability based on Li-Fi data and the observation probability based on laser data, wherein the observation probability model based on Li-Fi data is

An observation probability model based on laser data is

Wherein the content of the first and second substances,_tis white Gaussian noise observed at time t, with mean 0 and variance Q_t，x_tIs the pose of the robot at time t, z_t,1Is the observed quantity p (z) of the robot at the time t based on the Li-Fi optimal link_t,1|x_t) Is a pose x_tFor observed quantity z_t,1The observation probability of (2);k is the serial number of the laser observation data collected by the laser sensor,laser observation data, z, of number K acquired at time t_t，2Is the laser data set observed by the robot at time t, m is the occupancy probability map, p (z)_t,2|x_tM) is a pose x_tFor observed quantity z_t,2The probability of observation of (2).

9. A laser SLAM device based on lamplight calibration information fusion, which is characterized by comprising a processor and a memory, wherein the memory is stored with a computer program, and the computer program is executed by the processor to realize the laser SLAM method based on lamplight calibration information fusion according to any one of claims 1 to 8.

10. A computer-readable storage medium, on which a computer program is stored, wherein the computer program, when executed by a processor, implements the laser SLAM method based on light calibration information fusion according to any one of claims 1 to 8.

Technical Field

The invention relates to the technical field of laser SLAM and Li-Fi information transmission, in particular to a laser SLAM method and device based on lamplight calibration information fusion and a computer readable storage medium.

Background

The instant positioning and mapping technology (SLAM) is the basis and key for a mobile robot to search, detect, position and navigate and other tasks in an unknown environment, and the laser SLAM is used as a main body carrying a laser radar, so that environmental state information can be obtained in motion, the pose of the robot or the self can be estimated, and a surrounding environment map can be established and updated; the information processing mode of the SLAM can be divided into a filtering method and a graph optimization method; because the positioning, mapping and navigation algorithms of the two-dimensional laser SLAM are more mature than that of the visual SLAM, the two-dimensional laser SLAM is more suitable for indoor mobile robots; in fact, the external operating environment of the robot cannot be predicted, the uncertainty degrees of different environments are different, and the information acquired by the robot is also limited by the sensors, so that in an environment with unobvious geometric features or a complex environment, the mobile robot is difficult to know the accurate position and the surrounding environment of the mobile robot through a single sensor, and in order to enable the mobile robot to stably operate in the unknown environment, a laser SLAM scheme based on combination of multiple sensors becomes a new research hotspot.

In recent years, light emitting diodes have been widely used for area illumination due to their characteristics of long life, high luminance, and fast response. The visible Light communication technology (Li-Fi) is a new generation communication technology based on LEDs, and achieves the purpose of communication by rapidly switching on and off LEDs; an indoor positioning system based on the visible light communication technology, such as RSS, can realize indoor high-precision positioning without electromagnetic wave communication; in SLAM, loop detection is a recursive process, i.e. the current position of the robot is determined according to sensor information and compared with the previous position in the global map. Loop detection is a difficult point in the creation of an unknown environment map, for example, when a mobile robot runs in an environment with unobvious feature points such as a long corridor, the environment feature information obtained by a sensor is less, the difficulty of loop detection is increased, and the pose drift is too large; when the mobile robot carries out repeated judgment and probability estimation on the similar characteristic points, data to be processed is multiplied, and the accuracy of the established map is low.

Disclosure of Invention

In view of the above, the invention provides a laser SLAM method, device and computer readable storage medium based on lamplight calibration information fusion, and solves the technical problems of overlarge pose drift and lower accuracy of an established map in the prior art.

In one aspect, the invention provides a laser SLAM method based on lamplight calibration information fusion, which comprises the following steps:

acquiring a calibration coordinate of an LED, a code of an LED transmitter and an estimated coordinate of a receiver, and acquiring an optimal link according to the calibration coordinate of the LED, the code of the LED transmitter and the estimated coordinate of the receiver;

obtaining a current data frame based on Li-Fi according to the optimal link, and determining a homogeneous transformation matrix between frames based on Li-Fi according to the current data frame based on Li-Fi;

acquiring current laser frame data, acquiring data combination frame data according to the current laser frame data and an interframe homogeneous transformation matrix based on Li-Fi, registering the data combination data to obtain registered data combination frame data, and determining an interframe homogeneous transformation matrix based on laser according to the registered combination frame data;

and matching the current frame with a map and updating the map according to the pose transformation matrix between the Li-Fi and the laser data frame and the registered data combination frame data.

Further, obtaining the optimal link according to the calibration coordinates of the LED, the codes of the LED transmitter and the estimated coordinates of the receiver, specifically comprising establishing an optimal link mechanism model, and optimally linking the link through the optimal link mechanism model, wherein the optimal link mechanism model is

T_xi＝(x_i,y_i,i)，(x_i,y_j) For the calibration coordinates of the LED, i-1, 2,3 are the LED transmitter codes, R_xt＝(x_e,y_eT), t is the observation time, (x)_e,y_e) To estimate the coordinates of the receiver, E ═ 1,1,0)^T，

Is the number of Li-Fi links.

Further, acquiring data combination data according to current laser frame data and an interframe homogeneous transformation matrix based on Li-Fi, specifically comprising a step of obtaining data combination data according to the Li-Fi homogeneous transformation matrix T₁The method comprises the steps of carrying out mapping transformation on current laser frame data to obtain virtual frame data, carrying out spatial adjacent sequential preprocessing on the virtual frame data to obtain preprocessed virtual frame data, transforming the current laser frame data to a reference frame coordinate system to obtain reference frame data, and adding the reference frame data into the virtual frame data to obtain data combination frame data.

Further, converting the current laser frame data into a reference frame coordinate system to obtain reference frame data, specifically, converting the current laser frame data into the reference frame coordinate system through interframe homogeneous transformation, wherein a change relation of the interframe homogeneous transformation is

Wherein, T_j＝(x_j,y_j,θ_j)∈R²×[0,2π]Is a pose transformation matrix, when j is 1, T_jFor a Li-Fi-based inter-frame homogeneous transformation matrix, when j is 2, T_jFor a laser-based inter-frame homogeneous transformation matrix,represents a data point q_lBy transforming the matrix T_jConversion to q_l,f，f(T_j,q_l) For the corresponding mapping transformation function, q_l＝(q_lx,q_ly)^TAre the observed coordinates.

Further, determining a Li-Fi-based interframe homogeneous transformation matrix and a laser-based interframe homogeneous transformation matrix specifically comprises determining a Li-Fi-based interframe homogeneous transformation matrix T through a relative transformation solving formula₁Pose transformation matrix T corresponding to laser-based interframe homogeneous transformation₂The relative transformation solving formula is

Wherein the content of the first and second substances,

is the pose transformation matrix of the kth iteration,

represents a data point q_lTransformed matrixConversion to q_l,f，

Further, registering the data combination data, specifically including calculating the euclidean distance between the current data and the adjacent data, and taking the two data with the minimum euclidean distance as the registration data to realize the registration of the data combination data.

Further, acquiring a pose transformation matrix fusing the Li-Fi and the laser data frame according to the Li-Fi-based interframe homogeneous transformation matrix and the laser-based interframe homogeneous transformation matrix, and specifically acquiring the pose transformation matrix fusing the Li-Fi and the laser data frame according to the Li-Fi-based interframe homogeneous transformation matrix, the laser-based interframe homogeneous transformation matrix and a probability-based weight coefficient.

Further, the laser SLAM method based on the lamplight calibration information fusion also comprises the steps of obtaining the observation probability based on the Li-Fi data according to the observation probability model based on the Li-Fi data, obtaining the observation probability based on the laser data according to the observation probability model based on the laser data, and obtaining the weight coefficient based on the probability according to the observation probability based on the Li-Fi data and the observation probability based on the laser data, wherein the observation probability model based on the Li-Fi data is

An observation probability model based on laser data is

Wherein the content of the first and second substances,_tis white Gaussian noise observed at time t, with mean 0 and variance Q_t，x_tIs the pose of the robot at time t, z_t,1Is the observed quantity p (z) of the robot at the time t based on the Li-Fi optimal link_t,1|x_t) Is a pose x_tFor observed quantity z_t,1The observation probability of (2);

k is the serial number of the laser observation data collected by the laser sensor,laser observation data, z, of number K acquired at time t_t，2Is the laser data set observed by the robot at time t, m is the occupancy probability map, p (z)_t,2|x_tM) is a pose x_tFor observed quantity z_t,2The probability of observation of (2).

On the other hand, the invention also provides a laser SLAM device based on lamplight calibration information fusion, which comprises a processor and a memory, wherein the memory is stored with a computer program, and when the computer program is executed by the processor, the laser SLAM method based on lamplight calibration information fusion is realized according to any one of the technical schemes.

In another aspect, the present invention further provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the laser SLAM method based on the light calibration information fusion according to any of the above technical solutions is implemented.

Compared with the prior art, the invention has the beneficial effects that: acquiring an optimal link according to the calibration coordinates of the LED, the codes of the LED transmitter and the estimated coordinates of the receiver by acquiring the calibration coordinates of the LED, the codes of the LED transmitter and the estimated coordinates of the receiver; obtaining a current data frame based on Li-Fi according to the optimal link, and determining a homogeneous transformation matrix between frames based on Li-Fi according to the current data frame based on Li-Fi; acquiring current laser frame data, acquiring data combination frame data according to the current laser frame data and an interframe homogeneous transformation matrix based on Li-Fi, registering the data combination data to obtain registered data combination frame data, and determining an interframe homogeneous transformation matrix based on laser according to the registered combination frame data; acquiring a pose transformation matrix fused between Li-Fi and a laser data frame according to the interframe homogeneous transformation matrix based on Li-Fi and the interframe homogeneous transformation matrix based on laser, and matching a current frame with a map and updating the map according to the pose transformation matrix fused between Li-Fi and the laser data frame and data after registration combined with frame data; the pose drift is inhibited, and the accuracy of the established map is improved.

Drawings

Fig. 1 is a schematic flowchart of a laser SLAM method based on lamplight calibration information fusion according to embodiment 1 of the present invention;

FIG. 2 is a schematic diagram of a distance-area model according to embodiment 1 of the present invention;

fig. 3 is a schematic view of a robot according to embodiment 1 of the present invention;

fig. 4 is a diagram showing a relationship between the number of link links of Li-Fi and a positioning error of a robot obtained by RSS indoor positioning in embodiment 1 of the present invention;

FIG. 5 is an environment map created by the raw ontology and pIC method described in embodiment 1 of the present invention;

fig. 6 is an environment map created by the laser SLAM method based on the light calibration information fusion according to embodiment 1 of the present invention;

fig. 7 is a schematic diagram illustrating comparison between the loop precision and the recall ratio of the laser SLAM method and Cartographer method based on lamplight calibration information fusion according to embodiment 1 of the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.