Robot positioning method and system based on multi-sensor fusion and semantic map

Abstract

The invention provides a robot positioning method and system based on multi-sensor fusion and a semantic map. The method comprises the steps that a monocular camera is used for recognizing a semantic object, and the direction angle of the semantic object in a camera coordinate system is calculated; the laser radar point cloud is converted to a camera coordinate system, and accurate position information of the object is acquired in combination with semantic direction information; constructing an environment map with semantic annotation as priori knowledge; the current pose is calculated through semantic matching during navigation and compared with a system positioning result, and when the pose error is smaller than a threshold value, it is considered that robot positioning is correct; and otherwise, positioning again and recovering accurate positioning by taking the semantic calculation pose as a reference. The method has the advantages of low hardware cost, high environmental adaptability and the like, the positioning problem caused by dynamic interference and feature deficiency is effectively solved, the positioning precision and robustness of the robot in a complex environment are remarkably improved, and the method can be widely applied to a positioning navigation system of the robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot platform mapping, positioning and navigation, and more particularly to a robot positioning method and system based on multi-sensor fusion and semantic map. BACKGROUND

[0002] With the rapid development of robot technology, a precise and reliable positioning system has become the key to achieving autonomous navigation. However, in practical applications, whether in indoor or outdoor environments, robots face the challenge of declining positioning accuracy or even failure, which is particularly prominent in dynamic scenarios and feature-lacking environments.

[0003] In dynamic environments such as shopping malls, stations, and other areas with dense human flow, traditional laser radar-based positioning methods often struggle to cope. Due to the presence of a large number of moving people and objects in the environment, the matching results of laser point clouds are severely disturbed, leading to positioning deviations. More problematic is the case of degenerative environments, such as long corridors or open warehouses, which lack obvious geometric features, making it difficult for laser radars to find enough matching feature points for precise positioning. At this time, the system has to rely on the data of odometry and inertial measurement units, but the inherent cumulative errors of these sensors will continue to increase over time, eventually causing positioning failure.

[0004] To solve these problems, researchers have begun to explore positioning methods based on semantic information. Unlike relying solely on geometric features, semantic positioning uses objects with clear semantics in the environment as references, such as doors, windows, signs, etc., which are usually characterized by high stability and good recognition. Traditional semantic positioning schemes often use depth cameras to obtain the position information of semantic objects, but such devices not only have high costs, but also have limited performance in complex outdoor environments such as strong light. SUMMARY

[0005] The present application addresses the technical problems existing in the prior art and provides a robot positioning method and system based on multi-sensor fusion and semantic map. This method realizes real-time verification and error correction of the robot's pose through the identification and matching of semantic objects, effectively improving positioning accuracy and environmental adaptability.

[0006] According to a first aspect of the present application, a robot positioning method based on multi-sensor fusion and semantic map is provided, comprising the following steps: Constructing a semantic grid map, identifying semantic objects and extracting semantic text information; Converting polar coordinate data collected by the laser radar into point cloud data in the Cartesian coordinate system, and performing coordinate transformation to obtain laser point coordinates in the camera coordinate system; Obtaining the semantic object pair and the center pixel coordinates of the detection box, and calculating the azimuth angle of the semantic object in combination with the camera intrinsic parameters; The coordinates of the laser point in the camera coordinate system of the semantic object are transformed to the coordinates in the robot coordinate system using ROS TF tree. The distance of the semantic object relative to the robot is obtained, a spatial circle is drawn and the intersection points are selected, and the position of the robot is calculated. Draw a circle to filter out the camera coordinates and calculate the robot's orientation; The robot pose, including the robot's position and orientation, is compared with the pose of the localization algorithm. If the pose error is greater than a threshold, it indicates that the robot's position is lost, and the robot's position is transmitted to the localization algorithm for repositioning.

[0007] Based on the above technical solution, the present invention can also be improved as follows.

[0008] Optionally, the semantic text information includes: semantic object type and semantic object coordinates; the construction of the semantic raster map, identification of semantic objects, and extraction of semantic text information includes: The main control unit acquires environmental information through a lidar sensor and constructs a grid map of the environment using the Cartographer algorithm. The main control unit uses a deployed YOLO model to detect and acquire the azimuth angle of semantic objects in real time, and processes RGB images captured by the camera in real time to detect trained semantic objects and calculate the coordinates of points in the bounding box.

[0009] Optionally, the step of converting the polar coordinate data acquired by the lidar into point cloud data in the Cartesian coordinate system and performing coordinate transformation to obtain the laser point coordinates in the camera coordinate system includes: The rigid body transformation parameters of the lidar to the camera are obtained by using ROS TF tree. The point cloud coordinate system is registered using rotation matrix and translation vector, and the point cloud data is accurately mapped to the camera coordinate system. The coordinate transformation is then performed to obtain the coordinates of each laser point in the camera coordinate system.

[0010] Optionally, the step of performing coordinate transformation to obtain the laser point coordinates in the camera coordinate system further includes: Real-time TF transformation is obtained through ROS, and coordinate transformation calculation is performed to obtain the coordinates of each laser point in the camera coordinate system, thereby achieving alignment between the point cloud data and the camera coordinate system; the coordinate transformation calculation is represented as follows: Let the three-dimensional coordinates of the laser point in the lidar coordinate system be... The transformation matrices R and T obtained through ROS are used to calculate the coordinates in the camera coordinate system. :

[0011]

[0012] in, represents the laser point coordinates in the laser radar coordinate system; represents the converted coordinates in the camera coordinate system; R is a rotation matrix, representing the rotation difference between the laser radar coordinate system and the camera coordinate system; T is a translation vector, representing the position offset between the laser radar coordinate system and the camera coordinate system.

[0013] Optionally, the acquisition of the semantic object pair and the center pixel coordinates of the detection frame, combined with the camera intrinsic parameter, calculates the azimuth angle of the semantic object includes: The semantic object pair in the monocular camera image is detected using YOLO, and the center pixel coordinates of the detection frame are received from YOLO, combined with the camera intrinsic parameter to calculate the azimuth angle of the object, based on the registered laser radar point cloud data, the laser point coordinates of the semantic object pair in the camera coordinate system are calculated.

[0014] Optionally, the formula for calculating the azimuth angle of the semantic object combined with the camera intrinsic parameter is:

[0015] In the formula, represents the horizontal angle of the object in the camera coordinate system, center x is the center point coordinate of the semantic object, is the focal length of the camera in the x direction; is the principal point coordinate of the camera; through this azimuth angle, the master computer calculates the azimuth angle of the objects A and B in the camera coordinate system and .

[0016] Optionally, the conversion of the laser point coordinates of the semantic object in the camera coordinate system to the coordinates in the robot coordinate system through the ROS TF tree includes: By matching the points corresponding to the object pair in the laser radar point cloud data, the specific positions of the semantic objects A and B in the camera coordinate system are obtained , ; Through the TF tree, the transformation relationship between the camera coordinate system and the robot itself coordinate system is obtained, and the positions of the semantic objects A and B in the camera coordinate system , are converted to the positions in the coordinate system , ; Through the coordinates of the semantic objects A and B in the coordinate system, the distances of the semantic objects A and B from the XY plane of the robot , , wherein , .​

[0017] Optionally, the drawing of the space circle and the screening of the intersection point, the position of the robot comprises: For the semantic objects A, B detected by YOLO, the pixel coordinates of the centers of the semantic objects are obtained And ; If , it indicates that in the field of view of the camera, object A is on the left side of object B, so in the field of view of the camera, the direction from A to the camera O and then to B is counterclockwise; the cross product of the vector AO and the vector OB is taken, and if the vector AO x OB is greater than 0, it indicates that the vector AO is counterclockwise to the vector OB, and the intersection point O with the cross product greater than 0 is selected as the robot position coordinate If > , it indicates that in the field of view of the camera, object B is on the left side of object A, the cross product of the vector BO and the vector OA is taken, and if the vector BO x OA is greater than 0, it indicates that the vector BO is counterclockwise to the vector OA, and the intersection point O with the cross product greater than 0 is selected as the robot position coordinate.

[0018] Optionally, the drawing of the circle and the screening of the camera coordinates, the calculation of the orientation of the robot comprises: The position and the orientation of the camera are calculated; The orientation angle of the robot body in the world coordinate system is calculated according to the position and the orientation of the camera , the formula is as follows:

[0019] Among them, is an element of the rotation matrix R, , is the difference between the global azimuth angle and the local azimuth angle.

[0020] According to the second aspect of the present application, a robot positioning system based on multi-sensor fusion and semantic map is provided, comprising: A semantic grid map construction module is used to construct a semantic grid map, identify semantic objects and extract semantic text information; A coordinate conversion module is used to convert the polar coordinate data collected by the laser radar into point cloud data in the Cartesian coordinate system, and perform coordinate transformation to obtain laser point coordinates in the camera coordinate system; A azimuth angle calculation module of the semantic object is used to obtain the pixel coordinates of the semantic object pair and the detection frame center, and calculate the azimuth angle of the semantic object in combination with the camera intrinsic parameter; ​The robot pose calculation module is used to transform the coordinates of laser points in the camera coordinate system of semantic objects to the robot coordinate system using ROS TF trees, obtain the distance of semantic objects relative to the robot, draw a spatial circle and filter the intersection points to calculate the robot's position; and draw a circle to filter the camera coordinates and calculate the robot's orientation. The logic judgment and relocation module is used to compare the robot pose with the pose of the localization algorithm. If the pose error is greater than the threshold, it indicates that the robot position is lost, and the robot position is passed to the localization algorithm for relocation.

[0021] The technical effects and advantages of this invention are as follows: This invention provides a robot localization method and system based on multi-sensor fusion and semantic mapping. By innovatively integrating the ranging capability of lidar and the semantic recognition function of a monocular camera, a low-cost, high-precision semantic map construction and localization verification system is achieved. This method not only effectively solves the problems of traditional lidar localization being susceptible to interference in dynamic environments and lacking features in degraded scenarios, but also overcomes the limitations of depth camera solutions, such as high cost and poor environmental adaptability. Through accurate matching of semantic objects and real-time pose correction, the localization robustness and navigation reliability of robots in complex environments are significantly improved, while greatly reducing the system hardware cost, providing a cost-effective and efficient localization solution for robots. Attached Figure Description

[0022] Figure 1 A flowchart illustrating the steps of a robot localization method based on multi-sensor fusion and semantic mapping provided in an embodiment of the present invention; Figure 2 The semantic object raster map provided in this embodiment of the invention; Figure 3 This is a schematic diagram of semantic object recognition provided in an embodiment of the present invention; Figure 4 This is a schematic diagram of laser data coordinate transformation provided in an embodiment of the present invention; Figure 5 This is a schematic diagram of target object angle calculation provided in an embodiment of the present invention; Figure 6 This is a schematic diagram of the cross product filtering intersection provided in an embodiment of the present invention; Figure 7 This is a schematic diagram of positioning verification provided in an embodiment of the present invention; Figure 8 The pose correction diagram is provided for an embodiment of the present invention. Detailed Implementation

[0023] With reference to the accompanying drawings, the technical solutions in the embodiments of the present application will be clearly and completely described below, obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0024] It needs to be explained that the embodiments of the present application propose a scheme of fusing lidar and monocular camera to solve the problems of positioning drift and failure in dynamic environment and degenerative scenarios of traditional laser positioning. The scheme makes full use of the robot's standard lidar and monocular camera, and through accurate sensor calibration, deeply fuses the lidar point cloud data and visual semantic information. Specifically, the monocular camera is responsible for identifying semantic objects in the environment and determining their positions, while the lidar provides accurate distance measurement. This complementary way not only greatly reduces the hardware cost, but also improves the adaptability of the system in various complex environments. Especially in degenerative scenarios where features are missing, the system can establish virtual constraints through the spatial relationship between semantic objects, effectively suppress the cumulative error of sensors, and ensure the continuity and reliability of positioning.

[0025] This scheme of fusing lidar and monocular camera realizes high-precision positioning correction through semantic object recognition and matching, providing a new technical path for the stable operation of robots in complex environments. It not only solves the limitations of traditional schemes in dynamic and degenerative environments, but also achieves better cost performance through optimized hardware configuration, having important practical value and promotion prospects.

[0026] It can be understood that based on the defects in the background art, the embodiments of the present application propose a robot positioning method based on multi-sensor fusion and semantic map, as shown in Figure 1 The method comprises the following steps: S1. Construct a semantic grid map, identify semantic objects and extract semantic text information; In this embodiment, the semantic text information includes the type of semantic objects and the coordinates of semantic objects. Figure 2 The semantic object grid map is shown in the figure; the construction of the semantic grid map includes: the host computer acquires environmental information through the lidar sensor, and constructs a grid map of the environment using the Cartographer algorithm; the host computer detects the azimuth angle of the semantic object in real time through the deployed YOLO model, and processes the RGB image collected by the camera in real time to detect the trained semantic object.

[0027] When the semantic object is detected, the host computer receives information including the category information of the object and the position of the semantic object in the image (i.e. the bounding box). The point coordinates of the bounding box are calculated the coordinates of the semantic object center in the image.

[0028] S2. Convert the polar coordinate data collected by the laser radar into point cloud data in the Cartesian coordinate system, and perform coordinate transformation to obtain the laser point coordinates in the camera coordinate system; The host computer converts the polar coordinate data collected by the laser radar into point cloud data in the Cartesian coordinate system, representing the real physical distance in the x-y plane. In this embodiment, converting the polar coordinate data collected by the laser radar into point cloud data in the Cartesian coordinate system includes: The rigid transformation parameters of the laser radar to the camera are obtained through the ROS TF tree, the registration of the point cloud coordinate system is completed by using the rotation matrix and the translation vector, the point cloud data is accurately mapped to the camera coordinate system, the coordinate transformation is performed to obtain each laser point coordinate in the camera coordinate system, the alignment of the point cloud data and the camera coordinate system is completed, and the spatio-temporal alignment of the multi-modal perception data is realized.

[0029] The original data collected by the laser radar is usually represented in polar coordinates. The polar coordinate system uses two parameters to describe the position of a point in space: distance r and angle θ. Among them, r is the straight-line distance between the laser radar and the object, and θ is the angle measured along the clockwise direction from the starting scanning direction of the laser radar.

[0030] In order to convert the polar coordinate data of the laser radar into point cloud data in the Cartesian coordinate system, so that the point cloud has a clear position in the x-y plane, the host computer converts the polar coordinate data of the laser radar, and the conversion formula is:

[0031] Among them, x and y respectively represent the position of the point in the x-axis and y-axis of the Cartesian coordinate system. And the value of x is the real physical distance of the laser point in the x-axis, and the value of y is the real physical distance of the laser point in the y-axis. For two-dimensional laser, z is set to 0, indicating that all points are located in the same plane.

[0032] Through this conversion, the polar coordinate data (r, θ) of the laser radar is converted into point cloud data (x, y, z) in the Cartesian coordinate system. In order to convert the point cloud data collected by the laser radar into the camera coordinate system, the host computer obtains the spatial transformation relationship between the laser radar coordinate system and the camera coordinate system through the TF tree in the ROS system. This transformation is composed of a rotation matrix R and a translation vector T, which describes the rigid transformation between the two coordinate systems.

[0033] Let the three-dimensional coordinates of the laser point in the laser radar coordinate system be The transformation matrix R and T obtained by the host computer through ROS are calculated to obtain the coordinates in the camera coordinate system :

[0034]

[0035] wherein, represents the laser point coordinates in the laser radar coordinate system; represents the converted coordinates in the camera coordinate system; R represents the rotation matrix, representing the rotation difference between the laser radar coordinate system and the camera coordinate system; T represents the translation vector, representing the position offset between the laser radar coordinate system and the camera coordinate system.

[0036] The host computer obtains real-time TF transformation through ROS in this process, performs coordinate transformation calculation, obtains each laser point coordinate in the camera coordinate system, and realizes the alignment of the point cloud data and the camera coordinate system.

[0037] S3. Obtain the semantic object pair and the detection box center pixel coordinates, and calculate the azimuth angle of the semantic object in combination with the camera intrinsic parameter; In this embodiment, the host computer uses YOLO to detect semantic objects in the monocular camera image while the Cartographer algorithm constructs the grid map, and calculates the azimuth angle of the object through the camera intrinsic parameter. Specifically, the host computer detects semantic objects in the environment in real time, receives the detection box center pixel coordinates transmitted by YOLO, calculates the azimuth angle of the object in combination with the camera intrinsic parameter, calculates the laser point coordinates of the semantic object pair in the camera coordinate system based on the registered laser radar point cloud data, and calculates the laser point coordinates of the semantic object pair in the camera coordinate system based on the registered laser radar point cloud data; As Figure 2 the host computer calculates the position of the semantic object in the camera coordinate system, and calculates the azimuth angle of the object in combination with the camera intrinsic parameter.

[0038] The host computer first reads the stored semantic objects and their map coordinate data from the text file, and combines these objects into object pairs two by two, removes object pairs that are too far apart, and ensures that only object pairs that are relatively close are retained. This is a preprocessing stage; Then, when the robot detects a semantic object pair, the host computer will filter out object pairs with low confidence according to the confidence of each object pair. Next, the host computer will filter the semantic object pairs according to the category information of the detected semantic object pairs and the semantic object pairs retained in the preprocessing stage, and find the object pairs that meet the requirements and their position information.

[0039] When the target objects A and B are detected by YOLO, the camera intrinsic parameter K is combined, which is usually:

[0040] wherein, and are the focal length of the camera in x and y direction respectively; and are the principal point coordinates of the camera With this intrinsic matrix, the host computer can calculate the azimuth angle of objects A and B in the camera coordinate system and . The specific formula is:

[0041] wherein, represents the horizontal angle of the object in the camera coordinate system, center x is the semantic object center point coordinate, is the focal length of the camera in x direction; is the principal point coordinates of the camera; through this azimuth angle, the host computer can determine the direction of the object, which is further used for subsequent point cloud data matching.

[0042] S4. Convert the laser point coordinates of the semantic object in the camera coordinate system to the robot coordinate system through the ROS TF tree to obtain the distance of the semantic object relative to the robot, draw a space circle and screen the intersection points to calculate the position of the robot; In this embodiment, based on the registered laser radar point cloud data, the position of the object is obtained, and the coordinates are converted to the robot coordinate system through the ROS TF tree to obtain the distance of the semantic object relative to the robot. In combination with the object world coordinates in the semantic marking file, a space circle is drawn and the intersection points are screened to determine the position of the robot in the world coordinate system.

[0043] The conversion of the laser point coordinates of the semantic object in the camera coordinate system to the robot coordinate system through the ROS TF tree to obtain the distance of the semantic object relative to the robot includes: By matching the points corresponding to the object in the laser radar point cloud data, the host computer can obtain the specific position of the semantic objects A and B in the camera coordinate system , ; The host computer obtains the transformation relationship between the camera coordinate system and the robot itself coordinate system through the TF tree, and converts the position of the objects A and B in the camera coordinate system , to the position in the coordinate system , ; Through the coordinates of the objects A and B in the coordinate system, the XY plane distance of the semantic objects A and B relative to the robot can be calculated , ; wherein, , .

[0044] The drawing space circle and the intersection point are filtered, and the position of the robot comprises: The master control machine calculates the distance from the semantic objects A and B detected by YOLO to the robot and , and draws two circles with the object A and the object B world coordinates and as the centers and their distances to the robot as the radii. The intersection point O of the two circles is the possible position of the robot. Since there may be two intersection points of the two circles, we need to further filter out the correct intersection point.

[0045] For the semantic objects A and B detected by YOLO, the master control machine can obtain the pixel coordinates of the semantic object centers and . If < , it indicates that in the field of view of the camera, object A is on the left side of object B, so in the field of view of the camera, the direction from A to the camera O and then to B is in the counterclockwise order. At this time, the cross product of the vector AO and the vector OB is taken, and if the vector AO x OB is greater than 0, it indicates that the vector AO is turned counterclockwise to the vector OB, and at this time, the condition is met. If > , it indicates that in the field of view of the camera, object B is on the left side of object A, and the cross product of the vector BO and the vector OA is used for judgment, and the intersection point O with the cross product greater than 0 is selected as the robot position coordinate.

[0046] S5. Draw a circle to filter out the camera coordinates and calculate the orientation of the robot; After the master control machine detects the semantic objects A and B in real time, the master control machine obtains the coordinates of the objects through the registered laser radar point cloud data, and obtains the distance information of the objects relative to the camera. Combined with the object world coordinates in the stored semantic marking file, the space circle is drawn and the intersection point of the two circles is filtered to determine the current position of the camera. At the same time, the azimuth angle of object A is calculated by using the pixel coordinates of the YOLO detection box center, and the global angle difference and the local angle difference between the camera and object A are calculated to obtain the orientation of the camera. Finally, through the ROS TF tree, the orientation angle of the robot body is calculated by using the orientation information of the camera.

[0047] The calculation of the orientation of the robot specifically comprises: First, the camera position is calculated: the master control machine combines the semantic objects A and B detected in real time with the object world coordinates and and the object-to-camera distance is obtained and Two circles are drawn with their radii as the object-to-camera distance .

[0048] Secondly, the camera orientation is calculated: the master computer calculates the object A center pixel coordinates detected by YOLO , combines the camera intrinsic matrix K, and calculates the azimuth angle of object A in the camera coordinate system , according to the world coordinates of object A , and the current position of the camera , the global azimuth angle of object A relative to the camera is calculated :

[0049] Therefore, the camera orientation angle is the difference between the global azimuth angle and the local azimuth angle: ; Finally, the robot orientation is calculated; the master computer obtains the fixed rigid transformation relationship (rotation matrix R and translation vector T) between the camera coordinate system and the robot body coordinate system According to the camera orientation angle , combined with the relative rotation relationship R of the two coordinate systems, the orientation angle of the robot body in the world coordinate system is calculated : wherein is the element of the rotation matrix R .

[0050] The master computer calculates the actual orientation of the robot .

[0051] S6. Obtain the robot pose, compare the robot pose with the localization algorithm pose, if the pose error is greater than the threshold value, it indicates that the robot position is lost, and the robot position is transmitted to the localization algorithm for repositioning.

[0052] The robot pose includes: the position and orientation of the robot; after detecting the semantic object pair, the master computer calculates the current pose of the robot according to the calculation, and compares the robot pose with the current pose output by the cartographer localization algorithm ) are compared; if the position difference in the comparison result exceeds a threshold of 30 cm, it is determined that the pose provided by the cartographer positioning algorithm is deviated. At this time, the host computer will start the braking mechanism and enable the repositioning module of the cartographer, and use the calculated pose as the current correct pose of the robot to reposition. After successful positioning, the host computer will remove the braking mechanism, and the robot will continue to execute the task.

[0053] It should be noted that Cartographer is an open source library for robot simultaneous localization and mapping (SLAM) provided by Google. It supports 2D and 3D SLAM tasks, and can construct a map in real time and determine the position of the robot in the map through sensor data (such as lidar, odometer, etc.) in an unknown environment. Cartographer realizes high-precision map construction and positioning function through efficient data processing and optimization algorithm. The Cartographer positioning algorithm is a real-time simultaneous localization and mapping (SLAM) algorithm. Its main goal is to generate an accurate three-dimensional map while locating the robot's position in that map using sensor data. The goal of the Cartographer positioning algorithm is to generate a high-precision map in real time and provide reliable positioning.

[0054] Figure 3 The identification semantic object schematic diagram provided by the embodiment of the application is provided; in the figure, the green box is the semantic object position, and the red dot is the semantic object center; Figure 4 The laser data coordinate conversion schematic diagram provided by the embodiment of the application is provided; the laser point data is converted to the camera coordinate system; Figure 5 The target object angle calculation schematic diagram provided by the embodiment of the application is provided; Figure 6 The cross-multiplication screening intersection schematic diagram provided by the embodiment of the application is provided; Figure 7 The positioning verification schematic diagram provided by the embodiment of the application is provided; the blue dot is the semantic object position, the red circle is the circle drawn with the semantic object as the center and the distance between the robot and the object as the radius, the black dot and the red arrow are the calculated robot pose, and the cyan dot and the green arrow are the robot pose under the positioning algorithm.

[0055] Figure 8 The pose correction diagram provided by the embodiment of the application is provided. The left figure is a robot position loss diagram, and the right figure is a corrected schematic diagram.

[0056] It needs to be explained that the hardware platform relied on by the algorithm includes: a robot platform chassis, a host computer, a laser radar, a monocular camera and an inertial navigation system; wherein the host computer is sequentially connected with the robot platform chassis, the laser radar, the monocular camera and the inertial navigation system.

[0057] According to the second aspect of the present application, a robot positioning system based on multi-sensor fusion and semantic map is provided, comprising: A semantic grid map construction module is configured to construct a semantic grid map, identify semantic objects and extract semantic text information. A coordinate conversion module is configured to convert polar coordinate data collected by the laser radar into point cloud data in the Cartesian coordinate system, and perform coordinate transformation to obtain laser point coordinates in the camera coordinate system. An azimuth angle calculation module of the semantic object is configured to obtain the pixel coordinates of the semantic object pair and the detection frame center, and calculate the azimuth angle of the semantic object in combination with the camera intrinsic parameters. A robot pose calculation module is configured to convert the laser point coordinates in the semantic object camera coordinate system to the robot coordinate system through the ROS TF tree, obtain the distance of the semantic object relative to the robot, draw a space circle and screen the intersection, and calculate the position of the robot; and draw a circle to screen the camera coordinates, and calculate the orientation of the robot. A logic judgment and repositioning module is configured to compare the robot pose with the pose of the positioning algorithm, and if the pose error is greater than a threshold value, it indicates that the robot position is lost, and the robot position is transmitted to the positioning algorithm for repositioning.

[0058] It can be understood that the robot positioning system based on multi-sensor fusion and semantic map provided by the present application corresponds to the robot positioning method based on multi-sensor fusion and semantic map provided by the foregoing embodiments, and the related technical features of the robot positioning system based on multi-sensor fusion and semantic map can refer to the related technical features of the robot positioning method based on multi-sensor fusion and semantic map, which will not be repeated here.

[0059] The device embodiments described above are only schematic, wherein the units illustrated as separate components can or can not be physically separated, and the components illustrated as units can or can not be physical units, i.e. they can be located in one place, or distributed on multiple network units. Part or all of the modules can be selected to achieve the purpose of the present embodiment scheme according to actual needs. Those skilled in the art can understand and implement without creative labor.

[0060] Those skilled in the art can clearly understand the implementation of the various embodiments by means of software and necessary general hardware platforms through the above description of the embodiments, and of course, the embodiments can also be implemented by hardware. Based on such understanding, the above technical solutions, essentially or in other words, the part that contributes to the prior art, can be embodied in the form of a software product. The computer software product can be stored in a computer readable storage medium, such as a ROM / RAM, a magnetic disk, an optical disk, and the like, and includes a number of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0061] Although the preferred embodiments of the present application have been described, those skilled in the art, once they know the basic inventive concept, can make additional changes and modifications to the embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the present application.

[0062] Obviously, those skilled in the art can make various modifications and variations to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalent technologies, the present application also intends to include these modifications and variations.

[0063] Finally, it should be noted that: the above only describes the preferred embodiments of the present application, and is not intended to limit the present application, although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement to some technical features, any modification, equivalent replacement, improvement, etc. within the spirit and principles of the present application, should be included in the protection scope of the present application.

Claims

1. A robot localization method based on multi-sensor fusion and semantic map, characterized in that, The method comprises the following steps: constructing a semantic grid map, identifying semantic objects and extracting semantic text information; converting polar coordinate data collected by the laser radar into point cloud data in a Cartesian coordinate system, and performing coordinate transformation to obtain laser point coordinates in a camera coordinate system; obtaining semantic object pair and detection frame center pixel coordinates, and calculating the azimuth angle of the semantic object in combination with the camera intrinsic parameter; converting the laser point coordinates of the semantic object in the camera coordinate system to coordinates in the robot coordinate system through the ROS TF tree, obtaining the distance of the semantic object relative to the robot, drawing a space circle and screening intersection points, and calculating the position of the robot; drawing a circle to screen out camera coordinates and calculating the orientation of the robot; comparing the robot pose including the position and orientation of the robot with the pose of the positioning algorithm, and if the pose error is greater than a threshold value, it indicates that the robot position is lost, and the robot position is transmitted to the positioning algorithm for repositioning. 2.The robot localization method based on multi-sensor fusion and semantic map according to claim 1, characterized in that, The semantic text information includes: semantic object categories and semantic object coordinates; the constructing a semantic grid map, identifying semantic objects and extracting semantic text information comprises: The host computer obtains environmental information through the laser radar sensor, and constructs a grid map of the environment using the Cartographer algorithm. The host computer detects the azimuth angle of the semantic object in real time through the deployed YOLO model, and processes the RGB image collected by the camera in real time to detect the trained semantic object and calculate the midpoint coordinates of the bounding box. 3.The multi-sensor fusion and semantic map based robot localization method according to claim 1, wherein, The conversion of the polar coordinate data collected by the laser radar into point cloud data in the Cartesian coordinate system, and the coordinate transformation to obtain the laser point coordinates in the camera coordinate system comprise: The rigid transformation parameters of the laser radar to the camera are obtained through the ROS TF tree, and the registration of the point cloud coordinate system is completed by using a rotation matrix and a translation vector, so that the point cloud data is accurately mapped to the camera coordinate system, and coordinate transformation is performed to obtain each laser point coordinate in the camera coordinate system.

4. The multi-sensor fusion and semantic map based robot localization method according to claim 3, characterized in that, The coordinate transformation to obtain the laser point coordinates in the camera coordinate system further comprises: The host computer obtains real-time TF transformation through ROS, performs coordinate transformation calculation, and obtains each laser point coordinate in the camera coordinate system, so as to realize the alignment of the point cloud data and the camera coordinate system; the coordinate transformation calculation is represented as follows: Let the three-dimensional coordinates of the laser point in the laser radar coordinate system be The transformation matrix obtained through the ROS is R and T, and the coordinates in the camera coordinate system are calculated as : wherein, represents the laser point coordinate in the laser radar coordinate system; represents the converted coordinate in the camera coordinate system; R is a rotation matrix, representing the rotation difference between the laser radar coordinate system and the camera coordinate system; T is a translation vector, representing the position offset between the laser radar coordinate system and the camera coordinate system. 5.The multi-sensor fusion and semantic map based robot localization method according to claim 1, wherein, The obtaining of the semantic object pair and the detection frame center pixel coordinates, and the calculation of the azimuth angle of the semantic object in combination with the camera intrinsic parameter comprise: The YOLO is used to detect the semantic object pair in the monocular camera image, and the detection frame center pixel coordinates transmitted by the YOLO are received, the azimuth angle of the object is calculated in combination with the camera intrinsic parameter, the laser point coordinates of the semantic object pair in the camera coordinate system are calculated based on the registered laser radar point cloud data.

6. The multi-sensor fusion and semantic map based robot localization method according to claim 5, wherein, The formula for calculating the azimuth angle of the semantic object in combination with the camera intrinsic parameter is represented as follows: wherein, denotes the horizontal angle of the object in the camera coordinate system, center x is the semantic object center point coordinate, is the focal length of the camera in the x direction; is the principal point coordinate of the camera, through which the azimuth angle of the objects A and B in the camera coordinate system is calculated and . 7.The multi-sensor fusion and semantic map based robot localization method according to claim 1, characterized in that, The conversion of the laser point coordinates of the semantic object in the camera coordinate system to coordinates in the robot coordinate system through the ROS TF tree comprises: By matching the points corresponding to the objects in the laser radar point cloud data, the specific positions of the semantic objects A and B in the camera coordinate system are obtained , ; Through the TF tree, the camera coordinate system and the robot itself are obtained The transformation relationship of the coordinate systems, the positions of the semantic objects A and B in the camera coordinate system , are converted into the positions in the robot coordinate system , ; By semantic object A, B in The coordinates of the coordinate system, the distance between semantic objects A and B and the XY plane of the robot is calculated , . 8.The multi-sensor fusion and semantic map based robot localization method according to claim 1, wherein, The drawing of a space circle and the screening of intersection points to calculate the position of the robot comprises: For the semantic objects A, B detected by YOLO, the pixel coordinates of the centers of the semantic objects are obtained and ; If , it is explained that object A is on the left side of object B in the field of view of the camera, so in the field of view of the camera, the direction from A to the camera O and then to B is counterclockwise; the cross product of the orientation vector AO and the vector OB, if the vector AO x OB is greater than 0, it means that the vector AO turns to the OB in the counterclockwise direction, and the intersection point O with the cross product greater than 0 is selected as the robot position coordinate​ If > , it is explained that under the camera view, object B is on the left side of object A, the cross product of the orientation vector BO and the vector OA, if the vector BO x OA is greater than 0, it is explained that the vector BO is counterclockwise to OA, and the intersection point O with the cross product greater than 0 is selected as the robot position coordinate. 9.The multi-sensor fusion and semantic map based robot localization method according to claim 1, wherein, The drawing of a circle to screen out camera coordinates and the calculation of the orientation of the robot comprise: calculating the position and orientation of the camera; Calculating an orientation angle of the robot body in the world coordinate system from the position and orientation of the camera , as follows: wherein are elements of the rotation matrix R, , is the difference between the global azimuth and the local azimuth.

10. A robot positioning system based on multi-sensor fusion and semantic map, characterized in that, comprise: a semantic grid map construction module for constructing a semantic grid map, identifying semantic objects and extracting semantic text information; A coordinate conversion module is configured to convert polar coordinate data collected by the laser radar into point cloud data in a Cartesian coordinate system, and perform coordinate transformation to obtain laser point coordinates in a camera coordinate system; An azimuth angle calculation module of the semantic object is configured to obtain pixel coordinates of the semantic object relative to a center of a detection frame, and calculate an azimuth angle of the semantic object in combination with camera intrinsic parameters; A robot pose calculation module is configured to convert the laser point coordinates in the semantic object camera coordinate system to coordinates in a robot coordinate system through a ROS TF tree, obtain a distance of the semantic object relative to the robot, draw a space circle, and screen intersection points to calculate a position of the robot; A circle drawing and screening module is configured to screen the camera coordinates and calculate a direction of the robot; A logic judgment and repositioning module is configured to compare the robot pose with a pose of a positioning algorithm, and if a pose error is greater than a threshold value, it is indicated that the robot position is lost, and the robot position is transmitted to the positioning algorithm for repositioning.

Images