A method for constructing a two-dimensional semantic map of indoor corners based on a robot platform

Abstract

The present invention proposes a method for constructing a two-dimensional semantic map of indoor corners based on a robot platform. The present invention processes the lidar combined with the Gmapping algorithm to obtain the environmental grid map and the real-time pose of the robot; trains a deep learning model through a semantic segmentation data set and corner target detection data to identify and predict indoor environmental corners and non-corner objects; at the same time, combines a depth camera to extract the semantic point clouds of corners and non-corner objects in the environment, and performs filtering based on statistical methods. The three-dimensional point cloud coordinates of non-corner objects after filtering and the three-dimensional point cloud coordinates of corners after filtering are respectively subjected to point cloud coordinate transformation in combination with the real-time pose of the robot to obtain their coordinates in the environmental grid map coordinate system, so as to construct an object grid map and synthesize a two-dimensional semantic map of indoor corners. The present invention has higher stability, provides richer and more reliable features for robot positioning and navigation, and thus can complete more advanced and complex tasks.

Description

Technical Field

[0001] The present invention belongs to the field of map building, positioning and navigation of a robot platform, and particularly relates to a method for constructing a two-dimensional semantic map of indoor corners based on a robot platform. Background Art

[0002] Map building is the first and crucial step for an intelligent robot platform to complete the positioning and navigation tasks. With the increasing requirements for the intelligence and automation of the robot platform, it is required that the robot platform can understand the geometric and semantic information contained in the surrounding environment. However, the topological map or grid map constructed by using the traditional laser SLAM method can only express the topological information and geometric information in the environment, lacking the extraction and description of the environmental semantic information, resulting in the robot platform being unable to truly understand the environment. Using visual SLAM can obtain environmental semantics, and it can obtain rich texture information, but it is greatly affected by light, the boundary is not clear enough, there is less texture in the dark, and the computing load is large, and there are cumulative errors in map building, which is not conducive to constructing a two-dimensional semantic map. At the same time, the movement of objects in the environment causes great changes in the established semantic map. Therefore, it is very meaningful to add the semantics of certain specific objects in the environment (such as corners) as the inherent features of the environmental map. Summary of the Invention

[0003] The present invention proposes a method for constructing an indoor two-dimensional semantic map containing corner information. The grid map is created by using the laser slam algorithm, and at the same time, a vision sensor and a deep learning model are combined for target recognition and detection, the semantic information of objects such as corners in the environment is extracted, and the method of Bayesian estimation is combined to perform incremental estimation on whether there are objects in the grid. The obtained laser map and object semantic map are fused with information, and the obtained semantic map is represented to obtain a two-dimensional semantic grid map containing corners. It will provide richer features for the positioning and navigation of the robot platform, so as to complete more advanced and complex tasks.

[0004] The specific technical solution adopted by the present invention to solve the problems existing in the prior art is a method for constructing a two-dimensional semantic map of indoor corners based on a robot platform.

[0005] The robot platform is characterized by including: a robot platform chassis, a main control machine, a lidar sensor, and a depth camera;

[0006] The robot platform chassis loads the main control machine, the lidar sensor, and the depth camera;

[0007] The main control machine is sequentially connected to the robot platform chassis, the lidar sensor, and the depth camera in a wired manner.

[0008] The method for constructing a two-dimensional semantic map of an indoor corner is characterized by including the following steps:

[0009] Step 1: The main control machine controls the chassis of the robot platform to drive the robot platform to move indoors. The lidar sensor collects the distance between indoor objects and the robot platform and the azimuth angle between indoor objects and the robot platform in real time, and transmits them to the main control machine. The main control machine processes the distance between indoor objects and the robot platform and the azimuth angle between indoor objects and the robot platform through the Gmapping algorithm to obtain an environmental grid map and the real-time pose of the robot platform;

[0010] Step 2: Construct a semantic segmentation data set on the main control machine. Use the semantic segmentation data set as the training set. Input each non-corner sample image in the semantic segmentation data set into the DeepLab v2 network for prediction to obtain the predicted non-corner semantic labels. Further combine the non-corner semantic labels to construct the loss function of the DeepLab v2 network, and obtain the optimized DeepLab v2 network through optimized training; construct a corner target detection data set. Use the corner target detection data set as the training set. Input each corner sample image in the corner target detection data set into the SSD network for prediction to obtain the predicted external rectangular border and the object type in the predicted external rectangular border. Further combine the object types in the predicted box and the corner marked box to construct the loss function of the SSD network, and obtain the optimized SSD network through optimized training;

[0011] Step 3: The main control machine uses the depth camera to obtain the color image from the perspective of the robot platform, inputs the perspective color image into the optimized DeepLab v2 network for prediction to identify the non-corner semantic labels in the perspective color image; pass the perspective color image through the optimized SSD target detection network to identify the predicted external rectangular border of the corner and the object type in the predicted external rectangular border of the corner in the perspective color image; perform coordinate transformation on the non-corner semantic labels in the perspective color image, the predicted external rectangular border of the corner in the perspective color image and the object type in the predicted external rectangular border of the corner to obtain the three-dimensional point cloud coordinates of the non-corner and the three-dimensional point cloud coordinates of the corner in sequence. Perform point cloud filtering on the three-dimensional point cloud coordinates of the non-corner and the three-dimensional point cloud coordinates of the corner respectively through a filter based on a statistical method to obtain the filtered three-dimensional point cloud coordinates of the non-corner and the filtered three-dimensional point cloud coordinates of the corner;

[0012] Step 4: The main control machine performs point cloud coordinate transformation on the filtered three-dimensional point cloud coordinates of the non-corner and the filtered three-dimensional point cloud coordinates of the corner respectively, combined with the real-time pose of the robot platform, to obtain the coordinates of the non-corner object in the environmental grid map coordinate system and the coordinates of the corner object in the environmental grid map coordinate system. Construct an object grid map through the coordinates of the non-corner object in the environmental grid map coordinate system and the coordinates of the corner object in the environmental grid map coordinate system;

[0013] Step 5: Repeat Step 3 to Step 4 until the master control machine controls the robot platform to complete the traversal of the indoor environment, so as to obtain a complete environmental grid map and a complete object grid map, and further merge the complete environmental grid map and the complete object grid map to obtain a two-dimensional semantic map of the indoor wall corners.

[0014] Preferably, the real-time pose of the robot platform in Step 1 is:

[0015] c ok =(x o,k , y o,k , θ o,k ), k ∈ [1, K]

[0016] where c o，k represents the real-time pose of the robot platform at the k-th moment, x o,k represents the x-axis coordinate of the robot platform in the environmental grid map coordinate system at the k-th moment, y o,k represents the y-axis coordinate of the robot platform in the environmental grid map coordinate system at the k-th moment, θ o,k represents the yaw angle of the robot platform at the k-th moment, that is, the angle with the positive direction of the x-axis, and K is the number of acquisition moments;

[0017] Preferably, the semantic segmentation data set in Step 2 is:

[0018] I = {data m (u, v), type m (u, v)}, m ∈ [1, M], u ∈ [1, U I , v ∈ [1, V I

[0019] where M represents the number of non-wall corner sample images in the semantic segmentation data set I, U represents the number of columns of each non-wall corner sample image in the semantic segmentation data set I, V represents the number of rows of each non-wall corner sample image in the semantic segmentation data set I, s represents the number of categories to which the pixels of each non-wall corner sample image in the semantic segmentation data set I belong, data m (u, v) represents the pixel at the u-th column and v-th row of the m-th non-wall corner sample image in the semantic segmentation data set I, and type m (u, v) represents the category to which the pixel at the u-th column and v-th row of the m-th non-wall corner sample image in the semantic segmentation data set I belongs;

[0020] The loss function of the DeepLab v2 network in Step 2 is the cross-entropy loss function;

[0021] The optimized DeepLab v2 network obtained by optimizing the training in Step 2 is:​

[0022] Minimize the cross - entropy loss function as the optimization objective,

[0023] Optimize the DeepLab v2 network through the SGD algorithm to obtain the optimized DeepLab v2 network;

[0024] The object - detection dataset described in step 2 is:

[0025]

[0026] p ∈ [1, P], x ∈ [1, X], y ∈ [1, Y], n ∈ [1, N p

[0027] where P represents the number of corner - sample images in the object - detection dataset, X represents the number of columns of each corner - sample image in the object - detection dataset, Y represents the number of rows of each corner - sample image in the object - detection dataset, P represents the number of outer - rectangle borders in the p - th corner - sample image in the object - detection dataset, s represents the category to which the pixel point belongs; data m (x, y) is the pixel at the x - th column and y - th row of the p - th corner - sample image in the object - detection dataset, represents the abscissa of the upper - left corner of the n - th outer - rectangle border in the p - th corner - sample image in the object - detection dataset, represents the ordinate of the upper - left corner of the n - th outer - rectangle border in the p - th corner - sample image in the object - detection dataset, represents the abscissa of the lower - right corner of the n - th outer - rectangle border in the p - th corner - sample image in the object - detection dataset, represents the ordinate of the lower - right corner of the n - th outer - rectangle border in the p - th corner - sample image in the object - detection dataset, represents the object type in the n - th outer - rectangle border of the p - th corner - sample image in the object - detection dataset;

[0028] The SSD network loss function described in step 2 consists of a log loss function for classification and a smooth L1 loss function for regression;

[0029] The optimized SSD network obtained through optimization training described in step 2 is:

[0030] Optimize the SSD network through the SGD algorithm to obtain the optimized SSD network;

[0031] Preferably, the semantic label of non - corner in the perspective color image described in step 3 is:

[0032] *w k ={*data(u, v) k ,*type s,k}​

[0033] Among them, *data(u, v) k represents the pixel information of the pixel at the u-th column and v-th row in the perspective color image at the k-th moment, and *type s,k represents the object category to which the pixel at the u-th column and v-th row in the perspective color image at the k-th moment belongs;

[0034] The predicted circumscribed rectangular border of the corner in the perspective color image described in step 3 and the object types in the predicted circumscribed rectangular border of the corner are:

[0035]

[0036] Among them, represents the position and its corner category of the n-th predicted circumscribed rectangular border at the u-th column and v-th row in the perspective color image at the k-th moment in the perspective color image;

[0037] Perform coordinate transformation processing on the semantic labels of non-corners in the perspective color image, the predicted circumscribed rectangular borders of corners in the perspective color image, and the object types in the predicted circumscribed rectangular borders of corners described in step 3:

[0038] Combined with *w k and *e k , obtain the pixel coordinate set of non-corner semantics in the pixel coordinate system of the perspective color image:

[0039] W k ={((i w , j w ), type(i w,b , j w )) b}, b ∈ [1, B]

[0040] Among them, B represents the total number of non-corner semantic pixel points in the current picture in the set; (i w , j w ) represents the pixel point at the i-th w row and j-th w column in the picture, and ((i w , j w ), type(i w , j w )) b represents that the pixel coordinate of the b-th semantic pixel point in the pixel coordinate set in the picture is (i w , j w ), and the pixel label is type(i w , j w );

[0041] Obtain the pixel coordinate set of corner semantics in the pixel coordinate system of the perspective color image:

[0042] E k = {((i e , j e ), Ty(i e , j e )) t}, t ∈ [1, T]

[0043] where T represents the total number of semantic pixel points of the corner in the current image in the set; (i e , j e ) represents the pixel point at the i-th row and j-th column in the image, and ((i e , j e ), Ty(i e , j e )) e e t represents that the pixel coordinate in the image of the t-th semantic pixel point in the pixel coordinate set is (i e , j e ), and the pixel label is Ty(i e , j e ); w w

[0044] The non-corner semantic coordinates (i w , j w ) obtained above in the current color image pixel coordinate system are used to obtain their depth information and camera calibration parameters and converted to the camera coordinate system, so as to obtain the three-dimensional point cloud coordinates of non-corners: b

[0045]

[0046] For the formula, (X w , Y w , Z w b ) are the three-dimensional point cloud coordinates of non-corners. For each pixel point coordinate (i w , j w ) corresponding in the non-corner semantic pixel coordinate set, (i w , j w ) indicates that the pixel point is located at the i-th row and j-th column in the current color image, d w w w w w is the depth value of the pixel coordinate (i w , j w ) in the depth image; c x , c y , f x , f y are the internal camera calibration parameters, c x , c yrespectively represent the number of horizontal and vertical pixels between the central pixel coordinates of the image and the pixel coordinates of the image origin, that is, the optical center. f x , f y are respectively the distances from the camera focus to the horizontal and vertical directions of the camera optical center;

[0047] Convert the semantic coordinates of the wall corner (i e , j e ) in the current color image pixel coordinate system obtained above, and use the obtained depth map to obtain depth information and camera calibration parameters to convert to the camera coordinate system, so as to obtain the three-dimensional point cloud coordinates of the wall corner: t Obtain the semantic point cloud coordinates of the wall corner in the camera coordinate system (X

[0048] , Y e , Z e , Z e ):

[0049]

[0050] Among them, (X e , Y e , Z e ) are the three-dimensional point cloud coordinates of the converted wall corner. Each pixel point coordinate (i e , j e ) in the set of wall corner semantic pixel coordinates, (i e , j e ) indicates that the pixel point is in the i e -th row and j e -th column of the current color picture. d e is the depth value of the pixel coordinate (i e , j e ) in the depth image; c x , c y , f x , f y are the internal parameters of camera calibration. c x , c y respectively represent the number of horizontal and vertical pixels between the central pixel coordinates of the image and the pixel coordinates of the image origin, that is, the optical center. f x , f y are respectively the distances from the camera focus to the horizontal and vertical directions of the camera optical center;

[0051] In step 3, the three-dimensional point cloud coordinates of non-wall corners (X w , Y w , Z w ) and the three-dimensional point cloud coordinates of wall corners (X e , Y e , Z e ) are respectively processed by a filter based on a statistical method for point cloud filtering:

[0052] After the filter based on the statistical analysis method removes the discrete point cloud from the point cloud data, the closest cluster of point cloud to the robot platform is extracted from the object point cloud, which is equivalent to extracting the outer contour point cloud of the object in the viewing angle, and the coordinates of the non-corner semantic point cloud after filtering (X' w , Y w ', Z' w ) and the coordinates of the corner semantic point cloud after filtering (X' e , Y e ', Z' e ) are obtained.

[0053] Preferably, the point cloud coordinate transformation described in step 4 is as follows:

[0054] The point cloud coordinates (X' w , Y w ', Z' w ) of the obtained non-corner semantic point cloud in the camera coordinate system are converted to the robot platform coordinate system (X robot,w , Y robot,w , Z robot,w ), and the conversion relationship is:

[0055]

[0056] The point cloud coordinates (X' e , Y e ', Z' e ) of the obtained corner semantic point cloud in the camera coordinate system are converted to the robot platform coordinate system (X robot,e , Y robot,e , Z robot,e ), and the conversion relationship is:

[0057]

[0058] In the formula, R 1 and T 1 are respectively the 3*3 rotation matrix and the 3*1 translation matrix between the Kinect v2 depth camera coordinate system and the mobile robot platform coordinate system, which are determined according to the installation position relationship of the depth camera on the robot platform, and 0 T is (0, 0, 0).

[0059] Through the above conversion, the coordinates of the non-corner semantic point cloud (X robot,w , Y robot,w , Z robot,w ) in the robot platform coordinate system and the coordinates of the corner semantic point cloud (X robot,e , Y robot,e , Z robot,e ) are obtained, and the set of corner and non-corner semantic point cloud coordinates R = {(XR,f , Y R,f , Z R,f ), where the subscript f represents the f-th semantic point in the set; for convenience of representation, the coordinates of the semantic point cloud in the following robot platform coordinate system are uniformly represented by (X R , Y R , Z R ).

[0060] Combined with the real-time pose (x o,k , y o,k , θ o,k ) of the robot platform obtained by using the Gmapping mapping algorithm, the coordinates (X R , Y R , Z R ) in the robot platform coordinate system are transformed to obtain the coordinates (X Wo , Y Wo , Z Wo ) in the world coordinate system as follows:

[0061] The conversion relationship between the mobile robot platform coordinate system and the world coordinate system is:

[0062]

[0063] In the formula, R 2 and T 2 are respectively the 3*3 rotation matrix and the 3*1 translation matrix between the mobile robot platform coordinate system and the real world coordinate system, and 0 T is (0, 0, 0).

[0064]

[0065]

[0066] Finally, the coordinate points (X Wo , Y Wo ) in the real world coordinate system are transformed to obtain the coordinates (X g , Y g ) in the object grid map coordinate system.

[0067]

[0068] In the formula, (X Wo , Y Wo ) represents the coordinate value of a certain semantic point in the world coordinate system, and (X g , Y g ) is the coordinate value of this point corresponding to the object grid map coordinate system. r represents the resolution of the object grid map unit, and ceil is the ceiling symbol.

[0069] Through the above series of coordinate transformations, the coordinate values (X g , Y g ) of each semantic point in the object grid map coordinate system are obtained, and the finally obtained semantic points are marked on the created object grid map. Different object types g s use different colors s for marking;

[0070] Preferably, the merging of the complete environmental grid map and the complete object grid map in step 5 is as follows:

[0071] Obtain the center of the complete environmental grid map, align the center of the complete environmental grid map with the center of the complete object grid map, traverse the coordinates of the non-corner objects in the environmental grid map coordinate system and the coordinates of the corner objects in the environmental grid map coordinate system, and add the corresponding marks at the corresponding positions to the environmental grid map. The center and attitude of the complete environmental grid map and the complete object grid map should be kept consistent, and finally an indoor corner two-dimensional semantic map is obtained.

[0072] The present invention has the following advantages:

[0073] The present invention uses a semantic segmentation deep learning model for object semantic segmentation, and can obtain accurate object semantics, so that the environmental grid map originally established only with laser information has semantic-level meanings. At the same time, combined with the SSD object detection model to identify the corners in the environment, and add the extracted corner semantics to the original semantic map. Compared with other object semantics in the map, the corners have higher stability, and their positions will not change easily for the environment, providing more abundant and reliable features for the positioning and navigation of the robot platform, so as to complete more advanced and complex tasks. Description of the Drawings

[0074] Figure 1 : It is a flow chart of the method of the present invention;

[0075] Figure 2 : It is a diagram of the experimental scene;

[0076] Figure 3 : It is a schematic diagram of object detection and segmentation of the robot platform;

[0077] Figure 4 : It is a depth map and its visualization diagram;

[0078] Figure 5 : It is a preliminary semantic extraction diagram of the chair;

[0079] Figure 6 : It is a two-dimensional mapping diagram of the point cloud of the hollow chair;

[0080] Figure 7 : It is the semantic mapping graph during misdetection;

[0081] Figure 8 : It is the effect diagram of eliminating "false semantics" by the incremental method;

[0082] Figure 9 : It is the semantic mapping map;

[0083] Figure 10 : It is the synthesized semantic grid map; Specific implementation manners

[0084] For the convenience of those of ordinary skill in the art to understand and implement the present invention, the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the embodiments described herein are only used to illustrate and explain the present invention and are not used to limit the present invention.

[0085] The following combines Figures 1 to 10 The specific implementation manner of the present invention is introduced. The first specific embodiment of the present invention is a method for constructing a two-dimensional semantic map of indoor corners based on a robot platform.

[0086] The robot platform is characterized by including: a robot platform chassis, a main control machine, a lidar sensor, and a depth camera;

[0087] The robot platform chassis loads the main control machine, the lidar sensor, and the depth camera;

[0088] The main control machine is respectively connected to the robot platform chassis, the lidar sensor, and the depth camera in sequence by a wired manner.

[0089] The main control machine selected is the Core M4I7-D mini host;

[0090] The robot platform chassis selected is the Arduino drive board chassis of the pibot brand;

[0091] The lidar sensor selected is the SICK lms111 lidar with stable performance;

[0092] The depth camera selected is the kinect v2.

[0093] The method for constructing a two-dimensional semantic map of indoor corners is characterized by including the following steps:

[0094] Step 1: The master control machine controls the robot platform chassis to drive the robot platform to move indoors. The lidar sensor real-time collects the distance between indoor objects and the robot platform and the direction angle between indoor objects and the robot platform, and transmits them to the master control machine. The master control machine processes the distance between indoor objects and the robot platform and the direction angle between indoor objects and the robot platform through the Gmapping algorithm to obtain the environmental grid map and the real-time pose of the robot platform;

[0095] The real-time pose of the robot platform in Step 1 is:

[0096] cok=(x o,k ,y o,k ,θ o,k ), k∈[1, K]

[0097] where, c o,k represents the real-time pose of the robot platform at the k-th moment, x o,k represents the x-axis coordinate of the robot platform in the environmental grid map coordinate system at the k-th moment, y o,k represents the y-axis coordinate of the robot platform in the environmental grid map coordinate system at the k-th moment, θ o,k represents the yaw angle of the robot platform at the k-th moment, that is, the angle with the positive direction of the x-axis, and K is the number of acquisition moments;

[0098] Step 2: Build a semantic segmentation data set on the master control machine. Use the semantic segmentation data set as the training set. Input each non-corner sample image in the semantic segmentation data set into the DeepLab v2 network for prediction to obtain the predicted non-corner semantic labels. Further combine the non-corner semantic labels to build the loss function of the DeepLab v2 network, and obtain the optimized DeepLab v2 network through optimized training; build a corner object detection data set, use the corner object detection data set as the training set, input each corner sample image in the corner object detection data set into the SSD network for prediction to obtain the predicted external rectangular border and the object type in the predicted external rectangular border. Further combine the object types in the predicted box and the corner marked box to build the loss function of the SSD network, and obtain the optimized SSD network through optimized training;

[0099] The semantic segmentation data set in Step 2 is:

[0100] I={data m (u, v), type m (u, v)}, m∈[1, M], u∈[1, U I , v∈[1, V I

[0101] ​Among them, M represents the number of non-corner sample images in the semantic segmentation dataset I, U represents the number of columns of each non-corner sample image in the semantic segmentation dataset I, V represents the number of rows of each non-corner sample image in the semantic segmentation dataset I, s represents the number of categories to which the pixels of each non-corner sample image in the semantic segmentation dataset I belong, data m (u, v) represents the pixel at the u-th column and v-th row of the m-th non-corner sample image in the semantic segmentation dataset I, type m (u, v) represents the category to which the pixel at the u-th column and v-th row of the m-th non-corner sample image in the semantic segmentation dataset I belongs;

[0102] The DeepLab v2 network loss function described in step 2 is the cross-entropy loss function;

[0103] The optimized DeepLab v2 network obtained by optimizing the training in step 2 is:

[0104] Minimize the cross-entropy loss function as the optimization objective,

[0105] The optimized DeepLab v2 network is obtained by optimizing through the SGD algorithm;

[0106] The object detection dataset described in step 2 is:

[0107]

[0108] p ∈ [1, P], x ∈ [1, X], y ∈ [1, Y], n ∈ [1, N p

[0109] Among them, P represents the number of corner sample images in the object detection dataset, X represents the number of columns of each corner sample image in the object detection dataset, Y represents the number of rows of each corner sample image in the object detection dataset, P represents the number of outer rectangular borders in the p-th corner sample image in the object detection dataset, s represents the category to which the pixel point belongs; data m (x, y) is the pixel at the x-th column and y-th row of the p-th corner sample image in the object detection dataset, represents the abscissa of the upper left corner of the n-th outer rectangular border in the p-th corner sample image in the object detection dataset, represents the ordinate of the upper left corner of the n-th outer rectangular border in the p-th corner sample image in the object detection dataset, represents the abscissa of the lower right corner of the n-th outer rectangular border in the p-th corner sample image in the object detection dataset, represents the ordinate of the lower right corner of the n-th outer rectangular border in the p-th corner sample image in the object detection dataset, ​It represents the object type in the nth bounding rectangle of the pth corner sample image in the target detection dataset.

[0110] The loss function of the SSD network described in step 2 is composed of a log loss function for classification and a smooth L1 loss function for regression.

[0111] The optimized SSD network obtained through optimization training in step 2 is as follows:

[0112] The optimized SSD network is obtained by optimizing through the SGD algorithm.

[0113] Step 3: The master control machine uses a depth camera to obtain a color image from the perspective of the robot platform, inputs the perspective color image into the optimized DeepLab v2 network for prediction to identify the semantic labels of non-corner areas in the perspective color image; inputs the perspective color image into the optimized SSD object detection network to identify the predicted bounding rectangles of corners in the perspective color image and the object types in the predicted bounding rectangles of corners; performs coordinate transformation on the semantic labels of non-corner areas in the perspective color image, the predicted bounding rectangles of corners in the perspective color image, and the object types in the predicted bounding rectangles of corners to obtain the 3D point cloud coordinates of non-corner areas and the 3D point cloud coordinates of corners in sequence, and respectively performs point cloud filtering on the 3D point cloud coordinates of non-corner areas and the 3D point cloud coordinates of corners through a filter based on statistical methods to obtain the filtered 3D point cloud coordinates of non-corner areas and the filtered 3D point cloud coordinates of corners.

[0114] The semantic labels of non-corner areas in the perspective color image described in step 3 are:

[0115] *w k ={*data(u,v) k ,*type sk}

[0116] Among them, *data(u,v) k represents the pixel information of the u-th column and v-th row in the perspective color image at the k-th moment, and *type sk represents the object category to which the pixel of the u-th column and v-th row in the perspective color image at the k-th moment belongs.

[0117] The predicted bounding rectangles of corners in the perspective color image and the object types in the predicted bounding rectangles of corners described in step 3 are:

[0118]

[0119] Among them, represents the position of the nth predicted bounding rectangle of the u-th column and v-th row in the perspective color image at the k-th moment in the perspective color image and its corner category.

[0120] Perform coordinate transformation on the semantic labels of non-corner areas in the perspective color image, the predicted circumscribed rectangular frames of corners in the perspective color image, and the object types within the predicted circumscribed rectangular frames of corners as described in step 3:

[0121] Combine *w k and *e k to obtain the set of pixel coordinates of non-corner semantics in the pixel coordinate system of the perspective color image:

[0122] W k = {((i w , j w ), type(i w,b , j w )) b}, b ∈ [1, B]

[0123] where B represents the total number of non-corner semantic pixel points in the current image in the set; (i w , j w ) represents the pixel point at the i-th w row and j-th w column in the image, and ((i w , j w ), type(i w , j w )) b represents that the pixel coordinate of the b-th semantic pixel point in the pixel coordinate set in the image is (i w , j w ), and the pixel label is type(i w , j w );

[0124] Obtain the set of pixel coordinates of corner semantics in the pixel coordinate system of the perspective color image:

[0125] E k = {((i e , j e ), Ty(i e , j e )) t}, t ∈ [1, T]

[0126] where T represents the total number of corner semantic pixel points in the current image in the set; (i e , j e ) represents the pixel point at the i-th e row and j-th e column in the image, and ((i e , j e ), Ty(i e , j e )) tThe pixel coordinates in the image corresponding to the t-th semantic pixel point in the set of pixel coordinates are (i e ,j e ), and the pixel label is Ty(i e ,j e );

[0127] For the non-corner semantic coordinates (i w ,j w ) in the current color pixel coordinate system obtained above b , their depth information is obtained using the acquired depth map, and they are transformed into the camera coordinate system using the camera calibration parameters, thereby obtaining the 3D point cloud coordinates of the non-corner:

[0128]

[0129] The formula, (X w ,Y w ,Z w ) are the 3D point cloud coordinates of the non-corner. For each pixel point coordinate (i w ,j w ) in the non-corner semantic pixel coordinate set, (i w ,j w ) indicates that the pixel point is located in the i w -th row and j w -th column of the current color image. d w is the depth value of the pixel coordinate (i w ,j w ) in the depth image; c x , c y , f x , f y are the internal camera calibration parameters. c x , c y respectively represent the horizontal and vertical pixel numbers difference between the central pixel coordinate of the image and the pixel coordinate of the image origin, that is, the optical center. f x , f y are the horizontal and vertical distances from the camera focus to the camera optical center respectively;

[0130] For the corner semantic coordinates (i e ,j e ) in the current color pixel coordinate system obtained above t , their depth information is obtained using the acquired depth map, and they are transformed into the camera coordinate system using the camera calibration parameters, thereby obtaining the 3D point cloud coordinates of the corner:

[0131] Obtain the corner semantic point cloud coordinates (X e ,Y e ,Z e ) in the camera coordinate system:

[0132]

[0133] Among them, (X e , Y e , Z e ) are the three-dimensional point cloud coordinates of the corner obtained by conversion. For each pixel coordinate (i e , j e ) in the set of corner semantic pixel coordinates, (i e , j e ) indicates that this pixel is located in the i e -th row and j e -th column of the current color image. d e is the depth value of the pixel coordinate (i e , j e ) in the depth image; c x , c y , f x , f y are the internal parameters of camera calibration. c x , c y respectively represent the horizontal and vertical pixel numbers between the central pixel coordinate of the image and the pixel coordinate of the image origin, that is, the optical center. f x , f y are the horizontal and vertical distances from the camera focus to the camera optical center respectively;

[0134] The three-dimensional point cloud coordinates (X w , Y w , Z w ) of non-corner and the three-dimensional point cloud coordinates (X e , Y e , Z e ) of corner described in Step 3 are respectively processed by point cloud filtering through a filter based on statistical methods:

[0135] After removing the discrete point cloud from the point cloud data by the filter based on statistical analysis method, the cluster of point cloud closest to the robot platform is extracted from the object point cloud, which is equivalent to extracting the outer contour point cloud of the object under the view angle, and the filtered non-corner semantic point cloud coordinates (X' w , Y w ', Z' w ) and the filtered corner semantic point cloud coordinates (X' e , Y e ', Z' e ) are obtained.

[0136] Step 4: The master controller performs point cloud coordinate transformation on the filtered 3D point cloud coordinates of non-wall corners and the filtered 3D point cloud coordinates of wall corners respectively, combined with the real-time pose of the robot platform, to obtain the coordinates of the objects at non-wall corners in the environmental grid map coordinate system and the coordinates of the objects at wall corners in the environmental grid map coordinate system. An object grid map is constructed through the coordinates of the objects at non-wall corners in the environmental grid map coordinate system and the coordinates of the objects at wall corners in the environmental grid map coordinate system;

[0137] The point cloud coordinate transformation described in Step 4 is as follows:

[0138] Convert the obtained point cloud coordinates (X' w , Y w ', Z' w ) of the non-wall corner semantic point cloud in the camera coordinate system to the robot platform coordinate system (X robot,w , Y robot,w , Z robot,w ), and the conversion relationship is:

[0139]

[0140] Convert the obtained point cloud coordinates (X' e , Y e ', Z' e ) of the wall corner semantic point cloud in the camera coordinate system to the robot platform coordinate system (X robot,e , Y robot,e , Z robot,e ), and the conversion relationship is:

[0141]

[0142] In the formula, R 1 and T 1 are respectively the 3*3 rotation matrix and the 3*1 translation matrix between the Kinect v2 depth camera coordinate system and the mobile robot platform coordinate system, determined according to the installation position relationship of the depth camera on the robot platform, and 0 T is (0, 0, 0).

[0143] Through the above conversion, the coordinates (X robot,w , Y robot,w , Z robot,w ) of the non-wall corner semantic point cloud in the robot platform coordinate system and the coordinates (X robot,e , Y robot,e , Z robot,e ) of the wall corner semantic point cloud are obtained, and the coordinate set R of the wall corner and non-wall corner semantic point clouds is obtained as R = {(X R,f , Y R,f , Z R,f)}, where the subscript f represents the f-th semantic point in the set; for convenience of representation, the semantic point cloud coordinates in the following robot platform coordinate system are uniformly represented by (X R , Y R , Z R ).

[0144] Combined with the real-time pose (x o,k , y o,k , θ o,k ) of the robot platform obtained by using the Gmapping mapping algorithm, the coordinates (X R , Y R , Z R ) in the robot platform coordinate system are transformed to obtain the coordinates (X Wo , Y Wo , Z Wo ) in the world coordinate system as follows:

[0145] The transformation relationship between the mobile robot platform coordinate system and the world coordinate system is:

[0146]

[0147] In the formula, R 2 and T 2 are the 3*3 rotation matrix and the 3*1 translation matrix between the mobile robot platform coordinate system and the real world coordinate system respectively, and 0 T is (0, 0, 0).

[0148]

[0149]

[0150] Finally, the coordinate points (X Wo , Y Wo ) in the real world coordinate system are transformed to obtain the coordinates (X g , Y g ) in the object grid map coordinate system.

[0151]

[0152] In the formula, (X Wo , Y Wo ) represents the coordinate value of a certain semantic point in the world coordinate system, (X g , Y g ) is the coordinate value of this point corresponding to the object grid map coordinate system, r represents the object grid map cell resolution, and ceil is the ceiling symbol.

[0153] Through the above series of coordinate transformations, the coordinate values (X g , Yg ) and mark the finally obtained semantic points on the created object grid map, with different object types g s using different colors s for marking;

[0154] Step 5: Repeat steps 3 to 4 until the master control machine controls the robot platform to complete the traversal of the indoor environment, thereby obtaining a complete environmental grid map and a complete object grid map, and further merging the complete environmental grid map and the complete object grid map to obtain a two-dimensional semantic map of indoor corners;

[0155] The merging of the complete environmental grid map and the complete object grid map described in step 5 is as follows:

[0156] Obtain the center of the complete environmental grid map, align the center of the complete environmental grid map with the center of the complete object grid map, traverse the coordinates of the objects that are not corners and the coordinates of the objects at the corners in the coordinate system of the environmental grid map, and add the corresponding marks at that place to the corresponding positions of the environmental grid map. The center and pose of the complete environmental grid map and the complete object grid map should be kept consistent, and finally obtain a two-dimensional semantic map of indoor corners.

[0157] The second embodiment of the present invention is introduced below:

[0158] Step 1: With the help of the LMS111 lidar sensor and the mobile robot platform experimental platform in the real experimental scenario, use the Gmapping algorithm to complete the instant positioning of the robot platform and the creation of the grid map on the ROS operation platform. The experimental scenario diagram is as Figure 2 shown.

[0159] The robot platform moves in the environment to continuously improve the map. According to the odometer data and the odometer kinematic model obtained by the robot platform, estimate the pose of the robot platform at the current moment. Each particle contains the poses of all moments from the start of map building of the robot platform to the current moment and the current environmental map. Then, use the laser likelihood domain model to perform scan matching based on the laser data, calculate the matching degree between the map contained in the particle and the established map, perform weight update and resampling, update the map of the particle, and the pose of the particle with the highest score is the optimal pose. Obtain the position of the robot platform in real time and gradually generate an environmental grid map.

[0160] Step 2: Use the depth camera to obtain the view of the robot platform, and input the color image into the deep learning detection model to identify the target objects and corners in the field of view.

[0161] First, based on the principles of moderate and widespread data scenarios and balanced data categories and quantities, this paper creates indoor semantic segmentation datasets and indoor object detection datasets. Indoor scene images under multiple perspectives, distances, and brightness levels are collected, and images of the actual usage scenarios of the robot platform are added to form the INDOOR1 dataset for the indoor semantic segmentation task and the INDOOR2 dataset for the detection and recognition task in this paper. To further enrich the dataset and improve the generalization ability of the model, data augmentation operations such as color change, scale transformation, and random cropping are performed on the dataset before training. The DeepLab v2 and SSD networks are built, and the weights of the SSD network model are initialized with the network weights pre-trained on the ImageNet dataset. The model is trained using the prepared dataset in GPU mode.

[0162] During the process of the robot platform moving and mapping in the environment, the perspective color image obtained by the onboard depth camera is used as the input to the trained detection model to obtain the object detection and recognition results in the current perspective. For example, when the robot platform is in Figure 3 the scenario shown in (a) below, the detection effect of the corner is as shown in Figure 3 (d) below. The solid circles in the detection result image represent the positions of the detected corners in the picture. The detection and segmentation effects of the door and chair in the perspective are as shown in Figure 3 (c) below, where the non-black parts represent the detected door and cabinet respectively. It can be seen that the obtained detection effect is good, and the obtained results are used as the input for the subsequent semantic point cloud extraction.

[0163] Step 3: Combine the corresponding depth map in the same perspective to obtain the three-dimensional point cloud coordinates of the recognized corner in the camera coordinate system. For the generated point cloud data, corresponding filtering processing is performed to remove the false semantic point clouds.

[0164] After completing the object detection in the RGB image, we can obtain the category and position information of the objects in the two-dimensional image. By combining the depth map information, the actual distance information is obtained to achieve the conversion of three-dimensional information. The depth map corresponding to the scenario shown above is as shown in Figure 4 below.

[0165] After obtaining the correspondence between the color image and the depth, the point cloud (X K , Y K , Z K ) in the Kinect v2 camera coordinate system is obtained by Equation (1):

[0166]

[0167] In the formula, u and v are the pixel coordinate values of the objects segmented in the RGB image, d is the depth value of the pixel coordinate (u, v) in the depth image, and cx 、c y 、f x 、f y Calibrate the intrinsic parameters of the camera, which are the focal length and aperture center of the camera on two axes.

[0168] Through the above coordinate transformation, the 3D point cloud coordinates of the identified object in the camera coordinate system can be obtained.

[0169] Considering the misdetection of the training model and the computational complexity of point semantic generation, the initial 3D semantic point cloud is partially filtered out. Figure 5 The chair in the environment shown in (a) is segmented using the training model to obtain the color segmentation map (c). Due to its hollow structure, some point clouds in the segmentation map are actually point clouds on the wall behind the chair. If we directly perform the next step on all these point clouds, there will be a lot of false semantics in the obtained semantic information. Without any processing, these 3D point clouds are directly mapped to the 2D plane, and the effect is as follows Figure 6 As shown in (a), it can be seen from the mapping that there are some point clouds that obviously do not belong to the chair, and the object projection obtained appears to be rather messy. Therefore, it is necessary to perform corresponding filtering processing on the generated point cloud data.

[0170] Taking into account the range of the depth camera Kinect v2, there will be large discrepancies in the measurement results for being too close or too far. Here we set it to process only the point cloud within the range of 0.5 to 5 meters, and remove the point cloud that exceeds or is below the set range. To facilitate filtering, the remaining point cloud is reduced in dimension. It is first converted into a two-dimensional point cloud, and then a filter based on statistical analysis methods is used to remove noise points (outliers) from the point cloud data. The principle of this filter is: calculate the average distance from each point in the input point cloud set to all neighboring points. The result conforms to the Gaussian distribution, calculate the mean and variance, and remove those two-dimensional points far from the mean. After performing the above point cloud removal operation on the chair point cloud information in the figure above, the resulting two-dimensional mapping effect of the chair point cloud is as follows. Figure 6 As shown in (b).

[0171] It can be seen that the generated semantic information of the chair is more accurate. In order to better represent the semantic information of the extracted object, we extract the cluster of point clouds closest to the robot platform from the object point cloud, which is equivalent to extracting the outer contour point cloud of the object under the viewing angle. The specific implementation is: after obtaining the object semantic map, extract the grid points closest to the robot platform in each column of the image, and retain the point cloud for the subsequent coordinate system conversion operation. The extraction effect is as follows: Figure 6 Middle (c) figure.

[0172] After filtering the object point cloud coordinates in the camera coordinates, the mapped coordinates of the object's outer contour semantic point cloud in the camera plane are obtained.

[0173] Step 4: Using the real-time pose of the robot platform obtained by the Gmapping mapping algorithm, convert the semantic coordinates in the robot platform coordinate system to the world coordinate system, and then map them to the two-dimensional grid coordinate system according to the incremental estimation method, and represent them on the created object grid map;

[0174] The object semantic grid map is created simultaneously with the laser grid map. We first create a square blank grid map, with the center of the map as the starting point for mapping. It divides the environment into many grid cells of the same size, and its map resolution value is set the same as the mapping resolution value of the Gmapping algorithm. The difference is that the constructed object semantic grid map only contains the extracted object semantic mapping information, and the grids without recognized object projections are considered in the grid idle state. Considering the existing errors, the mapped coordinates of the same part of the same object detected at different times and positions on the map will be inconsistent. Here, we consider combining the historical state of the grid points for determination.

[0175] To minimize this error as much as possible, we use incremental estimation based on the Bayesian method to update the grid state.

[0176] For the grid map, use Q(s) to represent the grid state, which is determined by the probability p(s = 1) that the grid is occupied and the probability p(s = 0) that the grid is idle:

[0177]

[0178] When sensor data is received, a new model measurement value z is obtained. The state of the observation value has only two types (0 or 1). We need to update the grid state Q(s|z). From (2), we can get:

[0179]

[0180] From the above formula (3), we can get the probabilities that the grid is occupied and idle when the observation value is z:

[0181]

[0182] According to formulas (3) and (4), Q(s|z) can be calculated:

[0183]

[0184] Taking the logarithm on both sides of formula (5) gives:

[0185]

[0186] As can be seen from equation (6), only the first term of the grid probability value after state update is related to the measurement value, and the second term is the grid occupancy probability value before the update. We perform a similarity transformation on the above equation. The states before and after grid update are represented by Q t-1 and Q t respectively, and the model measurement value term is represented by Δq, resulting in the following equation (7):

[0187] Q t = Δq + Q t-1 (7)

[0188] When constructing the object semantic grid map using the above incremental estimation method, for the initial blank map, each grid corresponds to the initial state value Q 0 = 0. Different state update increments Δq are set according to the detection accuracy values of different types of objects, and a grid threshold D q is set. When the final grid state value is greater than D q , it proves that there is an object in this grid, and the occupied state of this grid will be updated and displayed in the constructed map. While updating the grid state of the object semantic mapping, the Bresenham line segment scanning algorithm is used to obtain all the grid points passed by the straight line connecting the semantic grid points to the grid points where the robot platform is located, and the state of these grid points is updated and all are set to the idle state.

[0189] During the map construction process, when the sensor data is updated, that is, when the pose of the robot platform changes, the above method is used to update the grid state, avoiding repeated use of the same frame of data as the measurement value for grid state update. Using the incremental estimation method can effectively eliminate the "false semantics" caused by misdetection and segmentation in some frames. For example, as shown in Figure 7 , when the robot platform recognizes the objects around it, the wooden block is mis-segmented as a door in a certain frame; if the recognized objects are directly used for semantic mapping, incorrect semantic information will exist in the resulting result, as shown in the effect of (c) in the figure. When combining the incremental estimation method for map construction, these misdetections caused by misrecognition in some frames will be eliminated. The robot platform is operated to perform multiple detections on the misrecognition location at different positions, so as to eliminate the probability value of the grid at the misrecognition location and obtain accurate object semantics. The effect diagram obtained during the process is as shown in Figure 8 .

[0190] Step 5: After the robot platform traverses the environment, obtain the object grid map and the environment grid map, and then merge the two grid maps to obtain a two-dimensional semantic grid map containing corner information that reflects the environmental semantic information;

[0191] After identifying the scanned grid points and their states in each frame of data, the above data is repeatedly obtained based on the pose states and sensor information of the mobile robot platform at different times. At the same time, according to the introduced Bayesian estimation principle, the state of each grid cell in the grid map coordinate system is updated. After the robot platform has traversed the usage environment, it is determined whether a certain type of object exists in a grid by whether the object probability value of each final grid is greater than a certain set threshold, thus completing the creation of the indoor object semantic grid map. For example, Figure 9 as shown, where Figure (a) is the obtained laser environment grid map, Figure (b) represents the object semantic mapping map in the environment, and Figure (c) represents the detected corner semantic map of the environment.

[0192] In the initially obtained corner semantic map, the corner projection points are relatively messy. We perform image morphological operations on it, conduct connected component analysis on the map, remove these small isolated point regions, connect the isolated points close to the main body part, and extract the small parts as the center of the corner point. Draw a solid circle of a certain size with it as the center to replace the corner point, and obtain the optimized corner semantic map as shown in Figure (b) of Figure 9 as shown.

[0193] Fusing the laser map and the object semantic map can obtain the composite semantic map as shown in Figure (b) of Figure 10 The specific merging process is as follows: Since the object grid map and the environment grid map are established synchronously, and the grid resolutions of the two maps are the same, the sizes of the same objects in the two generated maps are the same, and their directions are consistent. Therefore, the merging process of the two maps is convenient as long as the poses are aligned. First, use the map file generated by the environment grid map established by Gmapping to obtain the position of the map center (the origin of mapping); the generated object grid map is in picture format, and its center is the center of the picture. Just align the center of the environment grid map with the center of the object grid map and keep the direction unchanged, so that the centers of the two maps are aligned; then the map synthesis can be carried out. In fact, it can be regarded as a picture operation. OpenCV can be used as a tool to traverse and read the non-black areas (i.e., the target objects) of the object grid map, and add the colors of these areas to the corresponding positions in the environment grid map. During the process, the centers and poses of the two maps should be kept consistent. After traversing all the pixel values in the object grid map, the color areas in the environment grid map are also added. Finally, the generated environment grid map is the composite semantic grid map containing corner information.

[0194] To better express the high-level semantic information in the environment, some optimization processes are carried out on the constructed semantic map. Different-shaped colored blocks are used to replace different objects in a certain area of the environment. The result is as shown in Figure 10As shown in (c). Extract the central position of each object in the synthesized semantic map, store their coordinates in the map in the corresponding description file, and store them together with the constructed semantic map Figure 1 for subsequent use to query the positions of objects in the semantic map and complete higher-level positioning and navigation application tasks. It can be seen from the synthesized semantic map that the objects basically coincide with the corresponding obstacles in the grid map, the object positions are accurately expressed, the environmental semantic information can be correctly reflected, and the two-dimensional semantic grid map is created.

[0195] The protection scope of the present invention is not limited to the above embodiments. Obviously, those skilled in the art can make various changes and deformations to the present invention without departing from the scope and spirit of the present invention. If these changes and deformations fall within the scope of the claims of the present invention and their equivalent technologies, the intention of the present invention also includes these changes and deformations.

Claims

1. A method for constructing a two-dimensional semantic map of indoor corners based on a robot platform, characterized in that: The robot platform is characterized by including: a robot platform chassis, a main control unit, a lidar sensor, and a depth camera; The robot platform chassis loads the main control unit, the lidar sensor, and the depth camera; The main control unit is sequentially connected to the robot platform chassis, the lidar sensor, and the depth camera in a wired manner; The method for constructing a two-dimensional semantic map of indoor corners is characterized by including the following steps: Step 1: The main control unit controls the robot platform to move indoors. The lidar sensor real-time collects the distance between indoor objects and the robot platform and the direction angle between indoor objects and the robot platform, and transmits them to the main control unit. The main control unit processes the distance between indoor objects and the robot platform and the direction angle between indoor objects and the robot platform through the Gmapping algorithm to obtain an environmental grid map and the real-time pose of the robot platform; Step 2: Construct a semantic segmentation data set on the main control unit. Use the semantic segmentation data set as a training set. Input each non-corner sample image in the semantic segmentation data set into the DeepLab v2 network for prediction to obtain the predicted non-corner semantic labels. Further combine the non-corner semantic labels to construct a loss function for the DeepLab v2 network, and obtain an optimized DeepLab v2 network through optimized training; Construct a corner target detection data set. Use the corner target detection data set as a training set. Input each corner sample image in the corner target detection data set into the SSD network for prediction to obtain the predicted external rectangular border and the object type in the predicted external rectangular border. Further combine the object types in the predicted box and the corner marked box to construct a loss function for the SSD network, and obtain an optimized SSD network through optimized training; Step 3: The main control unit uses the depth camera to obtain a color image from the perspective of the robot platform, inputs the perspective color image into the optimized DeepLab v2 network for prediction to identify the non-corner semantic labels in the perspective color image; Pass the perspective color image through the optimized SSD target detection network to identify the predicted external rectangular border of the corner and the object type in the predicted external rectangular border of the corner in the perspective color image; Perform coordinate transformation processing on the non-corner semantic labels in the perspective color image, the predicted external rectangular border of the corner in the perspective color image, and the object type in the predicted external rectangular border of the corner to obtain the three-dimensional point cloud coordinates of the non-corner and the three-dimensional point cloud coordinates of the corner in sequence. Perform point cloud filtering processing on the three-dimensional point cloud coordinates of the non-corner and the three-dimensional point cloud coordinates of the corner respectively through a filter based on a statistical method to obtain the filtered three-dimensional point cloud coordinates of the non-corner and the filtered three-dimensional point cloud coordinates of the corner; Step 4: The master controller performs point cloud coordinate transformation on the filtered three-dimensional point cloud coordinates of non-corner points and the filtered three-dimensional point cloud coordinates of corner points respectively, combined with the real-time pose of the robot platform, to obtain the coordinates of non-corner objects in the environmental grid map coordinate system and the coordinates of corner objects in the environmental grid map coordinate system, and constructs an object grid map based on the coordinates of non-corner objects in the environmental grid map coordinate system and the coordinates of corner objects in the environmental grid map coordinate system; Step 5: Repeat Steps 3 to 4 until the master controller controls the robot platform to complete the traversal of the indoor environment, thereby obtaining a complete environmental grid map and a complete object grid map, and further merging the complete environmental grid map and the complete object grid map to obtain an indoor corner two-dimensional semantic map.

2. The method for constructing an indoor corner two-dimensional semantic map based on a robot platform according to claim 1, characterized in that the real-time pose of the robot platform in Step 1 is: c o,k = (x o,k , y o,k , θ o,k ), k ∈ [1, K] Among them, c o,k represents the real-time pose of the robot platform at the k-th moment, x o,k represents the x-axis coordinate of the robot platform in the environmental grid map coordinate system at the k-th moment, y o,k represents the y-axis coordinate of the robot platform in the environmental grid map coordinate system at the k-th moment, θ o,k represents the yaw angle of the robot platform at the k-th moment, that is, the angle with the positive direction of the x-axis, and K is the number of acquisition moments.

Images