Reinforcement learning type multi-sensor fusion navigation method, device and electronic equipment

Abstract

The application provides a reinforcement learning type multi-sensor fusion navigation method and device and electronic equipment, and relates to the technical field of data processing. The method comprises the following steps: acquiring point cloud data of a laser radar, image data of a binocular camera and IMU data of an inertial measurement unit at the same time; generating a visual landmark factor according to the image data; generating a straight line landmark factor and a plane landmark factor according to the point cloud data; generating an IMU pre-integration factor according to the IMU data; constructing a factor graph according to the visual landmark factor, the straight line landmark factor, the plane landmark factor and the IMU pre-integration factor; optimizing the factor weight by using reinforcement learning to generate an optimized target factor graph; and performing navigation according to the target factor graph. The device is used for executing the above method. According to the application, the factor graph is constructed according to the data collected by each sensor, the factor graph is optimized by using reinforcement learning, the optimized factor graph is used for navigation, and the navigation efficiency is improved under the condition that the calculation resource is limited.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of data processing, in particular to a reinforcement learning multi-sensor fusion navigation method and device and electronic equipment. BACKGROUND

[0002] Multi-modal sensor fusion is a key technology required for the successful deployment of autonomous navigation systems in real-world scenarios, which can be applied to scenarios such as autonomous underground exploration and outdoor dynamic mapping.

[0003] At present, in the case of limited computing resources, the data collected by multiple sensors is numerous and miscellaneous, which makes the navigation efficiency low in the application scenario of multi-sensor fusion. SUMMARY

[0004] The purpose of the embodiments of the present application is to provide a reinforcement learning multi-sensor fusion navigation method and device and electronic equipment to improve the computing efficiency in the process of constructing a factor graph, thereby improving the navigation efficiency of the mobile terminal.

[0005] In a first aspect, the embodiments of the present application provide a reinforcement learning multi-sensor fusion navigation method, which comprises: acquiring point cloud data of a laser radar, image data of a binocular camera and IMU data of an inertial measurement unit at the same time; generating a visual landmark factor according to the image data; generating a straight line landmark factor and a plane landmark factor according to the point cloud data; generating an IMU pre-integration factor according to the IMU data; constructing a factor graph according to the visual landmark factor, the straight line landmark factor, the plane landmark factor and the IMU pre-integration factor; the factor graph comprises a factor weight; optimizing the factor weight by using reinforcement learning to generate an optimized target factor graph; and navigating according to the target factor graph.

[0006] The embodiments of the present application synchronize the data of sensors such as laser radars, binocular cameras and inertial measurement units, and generate corresponding landmark factors according to the data collected by each sensor, and construct a factor graph according to each landmark factor. In this process, since the data collected by each sensor is time-aligned, the data of each sensor can be calculated at the same time, improving the computing efficiency. Moreover, the factor weight of the factor graph is optimized by using reinforcement learning. Since reinforcement learning can update parameters through a policy gradient algorithm, the mobile terminal integrated with multiple sensors such as laser radars, binocular cameras and inertial measurement units can adapt to more complex and diverse environments, thereby improving the navigation efficiency and further improving the universality of the mobile terminal.

[0007] In some embodiments, the straight line landmark factor is generated according to the point cloud data, including: segmenting the point cloud data into a plurality of point cloud clusters; calculating the curvatures of the point cloud data in each point cloud cluster; taking the point cloud data with the maximum curvature in each point cloud cluster as a straight line candidate point; wherein the plurality of straight line candidate points form a straight line candidate point set; and generating the straight line landmark factor according to the straight line candidate point set.

[0008] In the generation of the straight line landmark factor, the point cloud data collected by the laser radar is segmented, the curvatures of the point cloud data in the segmented point cloud clusters are calculated, the point cloud data that does not meet the requirements in each point cloud cluster is removed according to the curvatures, and the point cloud data that meets the requirements is taken as a straight line candidate point set for calculating the straight line landmark factor. In this process, since the entire point cloud data collected by the laser radar does not need to be calculated, the calculation efficiency is improved.

[0009] In some embodiments, the straight line landmark factor is generated according to the straight line candidate point set, including: obtaining a historical straight line feature point of a previous frame; predicting a predicted straight line feature point of a current frame according to the IMU data and the historical straight line feature point; screening the straight line candidate point set according to the predicted straight line feature point to generate a target straight line candidate point set within a preset distance range of the predicted straight line feature point; matching the predicted straight line feature point with the straight line feature points in the target straight line candidate point set, and taking the straight line feature points that match successfully as target straight line feature points; and generating the straight line landmark factor according to the target straight line feature points.

[0010] In the embodiments of the present application, after obtaining the straight line candidate point set, the predicted straight line feature point of the current frame is predicted according to the historical straight line feature point and the IMU data, the point cloud data in the straight line candidate point set is screened according to the predicted straight line feature point to obtain the target straight line feature point set, and the straight line landmark factor is generated according to the straight line feature points in the target straight line feature point set. Since only the straight line feature points near the predicted straight line feature point are matched and calculated, the calculation efficiency is further improved.

[0011] In some embodiments, the straight line landmark factor is: wherein, represents the state at time t, represents the predicted straight line feature, represents the transformation, represents the observed straight line feature, represents the coordinate system transformation operator under the straight line feature, represents the residual error operator under the straight line feature.

[0012] In some embodiments, the plane landmark factor is generated according to the point cloud data, including: segmenting the point cloud data into a plurality of point cloud clusters; calculating the curvatures of the point cloud data in each point cloud cluster; taking the point cloud data with the minimum curvature in each point cloud cluster as a plane candidate point; wherein the plurality of plane candidate points form a plane candidate point set; and generating the plane landmark factor according to the plane candidate point set.

[0013] In the generation of the plane landmark factor, the point cloud data collected by the laser radar is segmented, the curvatures of the point cloud data in the segmented point cloud clusters are calculated, the point cloud data that does not meet the requirements in each point cloud cluster is removed according to the curvatures, and the point cloud data that meets the requirements is taken as a plane candidate point set for calculating the plane landmark factor. In this process, since the entire point cloud data collected by the laser radar does not need to be calculated, the calculation efficiency is improved.

[0014] In some embodiments, the plane landmark factor is generated according to the plane candidate point set, including: obtaining a historical plane feature point of a previous frame; predicting a predicted plane feature point of a current frame according to IMU data and the historical plane feature point; screening the plane candidate point set according to the predicted plane feature point to generate a target plane candidate point set within a preset distance range of the predicted plane feature point; matching the predicted plane feature point with the plane feature points of the target plane candidate point set, and taking the plane feature points that pass the matching as target plane feature points; and generating the plane landmark factor according to the target plane feature points.

[0015] In the embodiments of the present application, after obtaining the plane candidate point set, the predicted plane feature point of the current frame is predicted according to the historical plane feature point and the IMU data, and the point cloud data in the plane candidate point set is screened according to the predicted plane feature point to obtain the target plane feature point set, so as to generate the plane landmark factor according to the plane feature points in the target plane feature point set. Since only the plane feature points near the predicted plane feature point are matched and calculated, the calculation efficiency is further improved.

[0016] In some embodiments, the plane landmark factor is: wherein, represents the state at time t, represents the predicted plane feature, represents the transformation, represents the observed plane feature, represents the coordinate system transformation operator under the plane feature, represents the residual error operator under the plane feature.

[0017] ​In a second aspect, the embodiments of the present application provide a multi-sensor fusion navigation device based on reinforcement learning, comprising: an acquisition module configured to acquire point cloud data of a laser radar, image data of a binocular camera and IMU data of an inertial measurement unit at the same time; a first generation module configured to generate a visual landmark factor according to the image data; a second generation module configured to generate a straight line landmark factor and a plane landmark factor according to the point cloud data; a third generation module configured to generate an IMU pre-integration factor according to the IMU data; a construction module configured to construct a factor graph according to the visual landmark factor, the straight line landmark factor, the plane landmark factor and the IMU pre-integration factor; the factor graph comprises factor weights; an optimization module configured to optimize the factor weights by using reinforcement learning to generate an optimized target factor graph; and a navigation module configured to perform navigation according to the target factor graph.

[0018] In a third aspect, the embodiments of the present application provide an electronic device, comprising: a processor, a memory, a storage medium and a bus, wherein the processor and the memory complete mutual communication through the bus; the memory stores program instructions executable by the processor, and the processor calling the program instructions can execute the method steps of the first aspect.

[0019] In a fourth aspect, the embodiments of the present application provide a non-transitory computer readable storage medium, comprising: the computer readable storage medium stores computer instructions, and the computer instructions make the computer execute the method steps of the first aspect.

[0020] Other features and advantages of the present application will be described in the following description, and some will become apparent from the description, or will be understood through implementation of the embodiments of the present application. BRIEF DESCRIPTION OF DRAWINGS

[0021] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings needed to be used in the embodiments of the present application. It should be understood that the following drawings only show some embodiments of the present application, and therefore should not be regarded as a limitation on the scope, and for those skilled in the art, other related drawings can also be obtained without creative labor on the basis of these drawings.

[0022] Figure 1 A flowchart of a multi-sensor fusion navigation method based on reinforcement learning provided by the embodiments of the present application;

[0023] Figure 2 A factor graph structure diagram provided by the embodiments of the present application;

[0024] Figure 3 A multi-sensor fusion navigation device structure diagram based on reinforcement learning provided by the embodiments of the present application;

[0025] Figure 4 An electronic device structure schematic diagram is provided for the embodiments of the present application. DETAILED DESCRIPTION

[0026] The embodiments of the technical solutions of the present application will be described in detail below with reference to the drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present application, and therefore only serve as examples, and cannot limit the protection scope of the present application.

[0027] It should be noted that all technical and scientific terms used herein have the same meaning as understood by those skilled in the art to which the present application belongs; the terms used herein are only for the purpose of describing the specific embodiments and are not intended to limit the present application; the terms “include” and “have” in the specification and claims of the present application and the above description of the drawings are intended to cover non-exclusive inclusion.

[0028] In the description of the embodiments of the present application, the technical terms “first”, “second”, etc. are only used to distinguish different objects, and cannot be understood as indicating or implying relative importance or implicitly indicating the number, specific order or primary and secondary relationship of the indicated technical features. In the description of the embodiments of the present application, the meaning of “a plurality of” is two or more, unless otherwise explicitly specified.

[0029] In the description of the embodiments of the present application, the term “and / or” is only used to describe the association relationship of the associated objects, which means that there can be three relationships, for example, A and / or B, which can represent the three cases of A alone, A and B together, and B alone. In addition, the character “ / ” herein generally represents an “or” relationship between the front and rear associated objects.

[0030] For the convenience of understanding, the sensor device involved in the technical solutions provided by the embodiments of the present application is introduced.

[0031] Laser radar is a radar system that detects the position, speed and other characteristic quantities of a target by emitting a laser beam. Its working principle is to emit a detection signal (laser beam) to the target, then compare the signal (target echo) received from the target reflection with the transmitted signal, and after appropriate processing, the relevant information of the target can be obtained, such as target distance, direction, height, speed, attitude, even shape, etc. It is composed of laser transmitter, optical receiver, turntable and information processing system, etc. The laser converts the electrical pulse into optical pulse and transmits it, and the optical receiver restores the optical pulse reflected from the target into electrical pulse and sends it to the display for information display.

[0032] Lidar contains several key parameters: wavelength, the laser light source used in ranging is mostly near-infrared band. Range: the light energy emitted by lidar is limited, there is a maximum detection distance, and the detection distance is greatly affected by the environment in actual use, the main influencing factors are: target object surface reflectivity, shape, environmental light interference, etc. Laser point cloud data rate, also known as sampling frequency, pulse frequency, is the number of laser beams that the laser can emit per unit time; under the same conditions, the higher the frequency, the more the number of detection points, the higher the work efficiency. Field of view angle range, the field of view angle (FOV) represents the scanning angle of the laser beam, which refers to the maximum angle range that the laser beam can reach through the scanning device, according to the field of view angle, the lidar can be divided into repetitive scanning lidar and non-repetitive scanning lidar, and the non-repetitive scanning lidar includes narrow-angle non-repetitive scanning lidar. Distance accuracy, the difference between the lidar ranging value and the true value; laser divergence angle, the laser emitted light has a divergence angle, which will produce a light spot. Multi-echo, due to the existence of the light spot, the lidar can receive multiple echoes, which can be used to detect the edges of the object. Under normal circumstances, the surface reflectivity of the measured target object will affect the measurement capability of the lidar, the factors affecting the surface reflectivity of the object mainly include the color and type of the object surface (matt, high brightness, smooth, rough, etc.). For example, a diffuse reflection object with rough surface such as a cement wall, the laser may be reflected in any direction, part of which will return to the lidar. For a mirror reflection object with a smooth surface such as a mirror, if the incident angle of the laser beam is small, the reflected beam of the laser beam cannot return to the receiver. The measurement value at this point will show no object, which will cause data loss.

[0033] Binocular camera is composed of two monocular cameras, which can obtain the spatial position of pixels through the distance between the cameras and can be widely used indoors and outdoors. Binocular camera contains several important parameters: disparity, since the binocular camera collects binocular images through left and right cameras, there is a certain disparity, which refers to the difference in the position of the same pixel on two images. Depth, which refers to the distance from the actual object to the camera, and the depth is equal to the distance between the two cameras multiplied by the focal length divided by the disparity. Epipolar plane is a plane composed of left and right cameras and three points of the object. Epipolar line, the intersection of the epipolar plane and the imaging plane. Epipolar constraint, if you want to find the corresponding points of the two images, you only need to find them on the epipolar line. The same pixel points in the two images must be on the same epipolar line.

[0034] The main components of an inertial measurement unit (IMU) are a gyroscope, an accelerometer, and a magnetometer. The gyroscope can obtain the acceleration of each axis, while the accelerometer can obtain the acceleration in the x, y, and z directions, and the magnetometer can obtain the information of the surrounding magnetic field. The main work of the inertial measurement unit is to fuse the data of the three elements to obtain more accurate attitude information.

[0035] Figure 1A flowchart of a reinforcement learning type multi-sensor fusion navigation method provided by an embodiment of the present application is shown in FIG. 1, which includes the following steps. Figure 1

[0036] In step 101, point cloud data of a laser radar, image data of a binocular camera and IMU data of an inertial measurement unit at the same time are acquired.

[0037] In the specific implementation process, the laser radar, the binocular camera and the inertial measurement unit can be installed on a mobile robot (such as a UAV, a sweeping robot, a logistics robot, etc.) terminal capable of autonomous positioning and navigation, or can be a mobile device with a positioning device held by a user. Taking the mobile robot terminal capable of autonomous positioning and navigation as an example, if the robot wants to detect the environmental information of a certain position, it can emit multiple laser beams to obtain point cloud data through the laser radar, obtain images through the binocular camera, and obtain IMU data through the inertial measurement unit, so as to analyze and estimate the surrounding environment according to the point cloud data, the image data and the IMU data.

[0038] Although the laser radar, the binocular camera and the inertial measurement unit are all mounted on the mobile robot, due to the different collection frequencies of the sensors, the collected data cannot be uniformly processed, so when collecting data, time alignment is needed to obtain the point cloud data of the laser radar, the image data of the binocular camera and the IMU data of the inertial measurement unit at the same time, so as to facilitate subsequent uniform processing of the data, reduce the data conversion process and improve the calculation efficiency.

[0039] In step 102, a visual landmark factor is generated according to the image data.

[0040] In the specific implementation process, the histogram equalization is performed on the image collected by the binocular camera to avoid the image being too bright or too dark. The feature points of the previous frame image are tracked on the current frame, and the tracked first feature points are retained. The other tracking failed points and points outside the image are eliminated. In order to ensure that the current frame feature points are uniformly distributed and the number is sufficient, a mask is set around the remaining tracked feature points, no new feature points are extracted in the mask, and second feature points are extracted outside the mask by using the corner detection algorithm (Features from accelerated segment test, FAST). Finally, the retained first feature points and second feature points are normalized and deformed to obtain the final image features, and the visual landmark factor is generated according to the image features.

[0041] The visual landmark factor is defined as the visual landmark The residual error at the state is:

[0042] ​

[0043] wherein, represents the state at time, represents a visual landmark point in Euclidean space, represents a platform pose, is a function that implements the projection of a visual landmark point into the image plane according to the platform pose . represents the detection result of the visual landmark point on the image plane. Given a pair of rectified binocular camera images, the residual of a binocular visual odometry is the difference between the measured landmark pixel position and the reprojected landmark pixel position and using a standard radial-tangential distortion model.

[0044] Step 103, generating a linear landmark factor and a planar landmark factor according to the point cloud data.

[0045] In the specific implementation process, linear features and planar features are extracted from the laser radar point cloud, a linear landmark factor is generated according to the linear features, and a planar landmark factor is generated according to the planar features. The processing flow of generating a linear landmark factor and a planar landmark factor according to the point cloud data includes: point cloud data de-distortion, point cloud data segmentation, point cloud data curvature calculation, and matching of linear features and planar features. For specific processes, please refer to the following embodiments, which will not be repeated here.

[0046] Step 104, generating an IMU pre-integration factor according to the IMU data.

[0047] In the specific implementation process, the standard way of currently measuring the pre-integration of the IMU is used to constrain the pose, velocity and bias between two consecutive nodes in the factor graph, and to provide high-frequency state updates between the nodes. The IMU pre-integration factor is defined as:

[0048]

[0049] wherein, represents the directional residual between two consecutive nodes in the factor graph, represents the linear velocity residual between two consecutive nodes in the factor graph, represents the position residual between two consecutive nodes in the factor graph, represents the IMU accelerometer bias residual between two consecutive nodes in the factor graph, represents the IMU gyroscope bias residual between two consecutive nodes in the factor graph.

[0050] Step 105: Construct a factor graph based on the visual landmark factor, the straight-line landmark factor, the planar landmark factor, and the IMU pre-integration factor; the factor graph includes factor weights.

[0051] In practice, factor weights are the proportions of factors, and can also be regarded as factor coefficients.

[0052] Figure 2 A schematic diagram of a factor graph structure provided in an embodiment of this application, such as... Figure 2 As shown, a factor graph is constructed based on visual landmark factors, straight-line landmark factors, planar landmark factors, and IMU pre-integration factors. In the factor graph, Indicates the robot's state. Indicates visual characteristics, Representing planar features, The black rectangles represent linear features, the black circles represent planar landmarks, the black rhombuses represent linear landmarks, the black triangles represent IMU pre-integration factors, and the black squares represent prior factors. Figure 2 It can be seen that each factor constrains and influences the robot's state.

[0053] Step 106: Optimize the factor weights using reinforcement learning to generate the optimized target factor graph.

[0054] Step 107: Navigate based on the target factor map.

[0055] In practical implementation, considering the complexity and diversity of the environment, this application provides a method for optimizing factor graphs using deep reinforcement learning. In the reinforcement learning model, the agent connects with the environment through perception and action. At each step of the interaction, the agent receives an observation of the current state of the environment as input, and then selects an action as output, which changes the state of the environment. The value of this state transition is conveyed to the agent through rewards. The agent's behavior should choose actions that tend to increase the sum of long-term reward values. It should be understood that in this application embodiment, the mobile robot is regarded as the agent interacting with the environment, and the mobile robot is hereinafter referred to as the system.

[0056] Before using reinforcement learning to optimize the factor graph, the state of the system is defined as follows:

[0057] The system state at any given time is defined as follows:

[0058]

[0059] in, As direction, is the position, is the linear velocity, is the bias of the IMU gyroscope and accelerometer.

[0060] In addition to the state, the system also tracks the visual features extracted by the binocular camera the line features of the lidar and the plane features . The system estimates the target is all the states up to the current time :

[0061]

[0062] In this way, the maximum a posteriori estimate is obtained as:

[0063]

[0064] The maximum a posteriori estimate can be expressed as a least squares problem:

[0065]

[0066] where, is the IMU measurement between and , is the index of all frames before . represents the IMU pre-integration factor, represents the weight of the IMU pre-integration factor; represents the plane landmark factor of the lidar, represents the weight of the plane landmark factor; represents the line landmark factor of the lidar, represents the weight of the line landmark factor; represents the visual landmark factor of the binocular camera, represents the weight of the visual landmark factor; represents the state prior factor, represents the weight of the prior factor.

[0067] In the reinforcement learning model, the action is the weight of various factors, the observation is the information perceived by the system from the sensor in the current state, and the reward is defined as the inverse of the relative pose error (RPE) between the estimated pose and the true pose. It should be noted that the sensor includes the lidar, the binocular camera and the inertial measurement unit (IMU).

[0068]

[0069] where, ​represents a sensor pose estimated by an odometry system; represents a sensor true pose. The subscript represents a current frame, and Δ represents a time interval.

[0070] It should be noted that the reinforcement learning model can be TD3 (Twin Delayed Deep Deterministic policy gradient algorithm), which is used to maximize long-term rewards. It can also be A3C (Asynchronous Advantage Actor-Critic), and it can also be DQN (Deep Q-Leaning Network), etc., which can be selected and determined according to actual conditions.

[0071] In the embodiments of the present application, the data of the laser radar, the binocular camera and the inertial measurement unit and the like are synchronously collected, and corresponding road marker factors are respectively generated according to the data collected by each sensor, and a factor graph is constructed according to each road marker factor. In this process, since the data collected by each sensor is time-aligned, the data of each sensor can be calculated at the same time, thereby improving the calculation efficiency. Moreover, the factor weights of the factor graph are optimized by using reinforcement learning. Since reinforcement learning can update parameters by using a policy gradient algorithm, the mobile terminal integrated with the laser radar, the binocular camera and the inertial measurement unit and the like can adapt to more complex and diverse environments, thereby improving the navigation efficiency and further improving the universality of the mobile terminal.

[0072] In some embodiments, generating a straight line road marker factor according to point cloud data comprises: segmenting the point cloud data into a plurality of point cloud clusters; calculating the curvatures of the point cloud data in each point cloud cluster; taking the point cloud data with the maximum curvature in each point cloud cluster as a straight line candidate point; wherein the plurality of straight line candidate points constitute a straight line candidate point set; and generating a straight line road marker factor according to the straight line candidate point set.

[0073] In the specific implementation process, since the laser point cloud can be captured by the laser radar during movement, it is necessary to remove the distorted point cloud data by using motion prior knowledge to remove the distorted point cloud data, and then to perform point cloud segmentation on the point cloud data without distortion, so as to obtain a plurality of point cloud clusters. It should be noted that when the number of point cloud data in the obtained point cloud cluster is less than a certain threshold, the point cloud cluster is marked as an abnormal value, and the point cloud cluster is removed. The certain threshold is a value defined according to actual conditions, which can be set to 6, 8, 10, etc., and the present application does not make a specific limitation thereto.

[0074] Curvature is a measure of the degree of bending of a curve. The curvature of each point cloud data in each point cloud cluster can be calculated based on covariance matrix, gradient calculation, kernel function filtering and curvature neighborhood least squares regression. After obtaining the curvature of each point cloud data in each point cloud cluster, the point cloud data with the maximum curvature in each point cloud cluster is taken as a straight line candidate point, and the straight line candidate points of each point cloud cluster form a straight line candidate point set, and then a straight line landmark factor is generated according to the straight line candidate point set.

[0075] In the process of generating a straight line landmark factor, the point cloud data collected by the laser radar is segmented, the curvature of each point cloud data in the segmented point cloud cluster is calculated, the point cloud data that does not meet the requirements in each point cloud cluster is removed according to the curvature, and the point cloud data that meets the requirements is taken as a straight line candidate point set for calculating a straight line landmark factor. In this process, since all the point cloud data collected by the laser radar does not need to be calculated, the calculation efficiency is improved.

[0076] In some embodiments, generating a straight line landmark factor according to the straight line candidate point set comprises: obtaining historical straight line feature points of a previous frame; predicting predicted straight line feature points of a current frame according to IMU data and the historical straight line feature points; screening the straight line candidate point set according to the predicted straight line feature points to generate a target straight line candidate point set within a preset distance range of the predicted straight line feature points; matching the predicted straight line feature points with the straight line feature points in the target straight line candidate point set, and taking the straight line feature points that match successfully as target straight line feature points; and generating a straight line landmark factor according to the target straight line feature points.

[0077] In the specific implementation process, the point cloud data obtained by one scan of the laser radar is regarded as one frame. After the straight line candidate point set is obtained by segmenting and calculating the curvature of one frame of point cloud data, the matching of the straight line feature is performed in the straight line candidate point set, so as to generate a straight line landmark factor according to the straight line feature. The straight line feature is composed of a plurality of straight line feature points.

[0078] It should be noted that one frame of data is obtained by one scan of the laser radar, the one frame of data received at the current time is a current frame, the data received before the current time is a historical frame, and the current frame includes a plurality of point cloud data.

[0079] In the specific matching process, the historical straight line feature points of the previous frame are obtained, and the plurality of historical straight line feature points form a historical straight line feature The IMU data collected by the inertial measurement unit and the straight line feature are used to predict the predicted straight line feature , wherein, The plurality of predicted straight line feature points form a predicted straight line feature.

[0080] According to Filter the set of candidate points for a straight line, and select the candidate points that match... Point cloud data points whose distance exceeds a preset threshold are discarded. Point cloud data within the preset distance range are used as a candidate set of target straight lines. The predicted straight line feature points are matched with the straight line feature points in the candidate set of target straight lines. The successfully matched straight line feature points are used as target straight line feature points, and straight line landmark factors are generated based on the target straight line feature points. It should be noted that the preset distance range is a pre-set value and can be set according to actual conditions. This application does not impose specific limitations on it.

[0081] The matching process is as follows: If the predicted linear feature is... Direction vector and the line features in the target line candidate point set Direction vector The included angle Less than the preset threshold And their center distance It is also less than the threshold. If the center distance is a straight line feature, then the match is considered successful. and straight line characteristics The difference between the distances from the origin and the given distance. The specific formula is as follows:

[0082]

[0083]

[0084] in, express directional vector, express directional vector, Represents a straight line The point closest to the origin. Represents a straight line The point closest to the origin.

[0085] It should be noted that after a line feature is successfully matched, its interior points are removed from the candidate line point set. This process is then repeated for the remaining line feature points in the candidate line point set until all line feature points in the candidate line point set are matched. After matching, multiple line features are obtained, which in turn yield multiple line landmark factors. An interior point refers to a point belonging to that line feature. It should be noted that one line feature includes multiple line feature points, and one line feature corresponds to one line landmark factor.

[0086] It should be noted that after matching is completed, the obtained straight line features can be subjected to Euclidean clustering, and the clustered results are taken as new straight line features, that is, a plurality of small straight line features are combined into one large straight line feature, and then the above process is performed on the new straight line features until the straight line features matched for the minimum continuous times are used to generate the straight line landmark factor.

[0087] The embodiment of the application obtains the predicted straight line feature point of the current frame according to the historical straight line feature point and the IMU data after obtaining the straight line candidate point set, and screens the point cloud data in the straight line candidate point set according to the predicted straight line feature point to obtain the target straight line feature point set, so as to generate the straight line landmark factor according to the straight line feature points in the target straight line feature point set. Since only the straight line feature points near the predicted straight line feature points are matched and calculated, the calculation efficiency is further improved.

[0088] In some embodiments, the straight line landmark factor is: wherein, represents the state at time t, represents the predicted straight line feature, represents the transformation, represents the observed straight line feature, represents the coordinate system transformation operator under the straight line feature, represents the residual error operator under the straight line feature.

[0089] In the specific implementation process, the straight line landmark factor is obtained by the following way:

[0090] The straight line can be parameterized by a rotation matrix R ∈ SO(3) and two scalars a, b ∈ R, = R is the direction of the straight line, and d = R(a +b ) represents the nearest point to the origin in the straight line, and a straight line can be defined as:

[0091]

[0092] Let be the operator that applies the transformation to all points in the straight line to obtain the straight line , wherein, represents a rotation transformation, represents a translation transformation, such that:

[0093]

[0094] ​

[0095]

[0096] two straight lines , The residual operator between the two straight lines is defined as:

[0097] log( ), , )

[0098] The residual between the straight line feature observed at the time and the predicted straight line feature is defined as follows:

[0099]

[0100] In some embodiments, the plane landmark factor is generated according to the point cloud data, including: segmenting the point cloud data into a plurality of point cloud clusters; calculating the curvatures of the point cloud data in each point cloud cluster; taking the point cloud data with the minimum curvature in each point cloud cluster as a plane candidate point; wherein the plurality of plane candidate points constitute a plane candidate point set; and generating the plane landmark factor according to the plane candidate point set.

[0101] In the implementation process, the segmentation and curvature calculation of the point cloud data are as described in the above embodiments, which will not be repeated here. The point cloud data with the minimum curvature in each point cloud cluster is taken as a plane candidate point, and the plane candidate points of each point cloud cluster constitute a plane candidate point set, and then the plane landmark factor is generated according to the plane candidate point set.

[0102] It should be noted that the segmentation and curvature filtering will generally reduce the number of point clouds in the point cloud data by 90%, thereby reducing a large amount of calculation in the subsequent plane feature and straight line feature processing.

[0103] In the generation of the plane landmark factor, the point cloud data collected by the laser radar is segmented, and the curvatures of the point cloud data in the segmented point cloud clusters are calculated, the point cloud data that does not meet the requirements in each point cloud cluster is removed according to the curvature, and the point cloud data that meets the requirements is taken as a plane candidate point set for calculating the plane landmark factor. In this process, since the entire point cloud data collected by the laser radar does not need to be calculated, the calculation efficiency is improved.

[0104] ​In some embodiments, generating planar landmark factors based on a set of planar candidate points includes: acquiring historical planar feature points from the previous frame; predicting predicted planar feature points for the current frame based on IMU data and historical planar feature points; filtering the set of planar candidate points based on the predicted planar feature points to generate a set of target planar candidate points within a preset distance range of the predicted planar feature points; matching the predicted planar feature points with the planar feature points in the set of target planar candidate points, and using the successfully matched planar feature points as target planar feature points; and generating planar landmark factors based on the target planar feature points.

[0105] In the specific implementation process, after segmenting and curvature calculation of a frame of point cloud data, a set of planar candidate points is obtained. Planar features are then matched within this set to generate planar landmark factors based on the planar features. Each planar feature consists of multiple planar feature points.

[0106] In the specific matching process, historical planar feature points from the previous frame are obtained, and multiple historical planar feature points constitute historical planar features. The prediction plane features of the current frame are predicted using IMU data collected by the inertial measurement unit and historical plane features. ,in, It consists of multiple feature points on the prediction plane.

[0107] according to Filter the set of candidate points in the plane, and select the candidate points that match... Point cloud data points whose distance exceeds a preset threshold are discarded. Point cloud data within the preset distance range are used as a candidate point set for the target plane. The predicted plane feature points are matched with the plane feature points in the candidate point set, and the successfully matched plane feature points are used as the target plane feature points. Plane landmark factors are generated based on the target plane feature points. It should be noted that the preset distance range is a pre-set value and can be set according to actual conditions. This application does not impose specific limitations on it.

[0108] The matching process is as follows: If the predicted planar features unit normal vector Planar features in the target plane candidate point set unit normal vector The angle between Less than ,and and The difference in distance to the origin Less than the threshold If the match is successful, the specific formula is as follows:

[0109]

[0110]

[0111] wherein, represents the signed distance between a point and a plane, represents the signed distance between a point and a plane. represents the signed distance between a point and a plane. The signed distance represents that the distance has a direction.

[0112] It should be noted that after the plane feature is successfully matched, the inlier points of the plane feature are removed from the plane candidate point set, and then the remaining plane feature points in the plane candidate point set are repeatedly matched according to the above process until all the plane feature points in the plane candidate point set are matched. After the matching is completed, a plurality of plane features are obtained, and a plurality of plane landmark factors are obtained.

[0113] It should be noted that after the matching is completed, the obtained plane features can be subjected to normal-based region growing to combine a plurality of small plane features into a large plane feature, so as to obtain new plane features, and then the above process is performed on the new plane features until the plane features matched for the minimum continuous times are used to generate the plane landmark factors.

[0114] In the embodiments of the present application, after the plane candidate point set is obtained, the predicted plane feature points of the current frame are predicted according to the historical plane feature points and the IMU data, and the point cloud data in the plane candidate point set is screened according to the predicted plane feature points to obtain a target plane feature point set, so as to generate the plane landmark factors according to the plane feature points in the target plane feature point set. Since only the plane feature points near the predicted plane feature points are matched and calculated, the calculation efficiency is further improved.

[0115] In some embodiments, the plane landmark factor is: wherein, represents the state at time t, represents the predicted plane feature, represents the transformation, represents the observed plane feature, represents the coordinate system transformation operator under the plane feature, represents the residual error operator under the plane feature.

[0116] In the specific implementation process, the plane landmark factor is obtained by the following method:

[0117] A plane is represented by a unit normal vector and the distance d between the plane and the origin:

[0118] ​​

[0119] Let be the homogeneous transformation of all points in the plane , and let be the operator that defines the error between two planes

[0120]

[0121] where B p ∈R 3×2 is a basis of the tangent space of i , and let be defined as follows:

[0122]

[0123] The residual between the observed plane feature at time and the predicted plane feature is defined as follows:

[0124]

[0125] Figure 3 A reinforcement learning multi-sensor fusion navigation device structure provided by an embodiment of the present application is shown in FIG. 1. The device includes an acquisition module 301, a first generation module 302, a second generation module 303, a third generation module 304, a construction module 305, an optimization module 306, and a navigation module 307. The acquisition module 301 is configured to acquire point cloud data of a laser radar, image data of a binocular camera, and IMU data of an inertial measurement unit at the same time. The first generation module 302 is configured to generate a visual landmark factor according to the image data. The second generation module 303 is configured to generate a straight line landmark factor and a plane landmark factor according to the point cloud data. The third generation module 304 is configured to generate an IMU pre-integration factor according to the IMU data. The construction module 305 is configured to construct a factor graph according to the visual landmark factor, the straight line landmark factor, the plane landmark factor, and the IMU pre-integration factor. The factor graph includes factor weights. The optimization module 306 is configured to optimize the factor weights by using reinforcement learning to generate an optimized target factor graph. The navigation module 307 is configured to perform navigation according to the target factor graph. Figure 3

[0126] The acquisition module 301 is configured to acquire point cloud data of a laser radar, image data of a binocular camera, and IMU data of an inertial measurement unit at the same time. The first generation module 302 is configured to generate a visual landmark factor according to the image data. The second generation module 303 is configured to generate a straight line landmark factor and a plane landmark factor according to the point cloud data. The third generation module 304 is configured to generate an IMU pre-integration factor according to the IMU data. The construction module 305 is configured to construct a factor graph according to the visual landmark factor, the straight line landmark factor, the plane landmark factor, and the IMU pre-integration factor. The factor graph includes factor weights. The optimization module 306 is configured to optimize the factor weights by using reinforcement learning to generate an optimized target factor graph. The navigation module 307 is configured to perform navigation according to the target factor graph.

[0127] ​On the basis of the above-mentioned embodiments, the second generation module 303 is specifically configured to: divide the point cloud data into a plurality of point cloud clusters; calculate the curvatures of the point cloud data in each point cloud cluster; take the point cloud data with the maximum curvature in each point cloud cluster as a straight line candidate point; wherein the plurality of straight line candidate points constitute a straight line candidate point set; and generate a straight line road marker factor according to the straight line candidate point set.

[0128] On the basis of the above-mentioned embodiments, the second generation module 303 is specifically configured to: obtain a historical straight line feature point of a previous frame; predict a predicted straight line feature point of a current frame according to the IMU data and the historical straight line feature point; perform screening on the straight line candidate point set according to the predicted straight line feature point, to generate a target straight line candidate point set within a preset distance range of the predicted straight line feature point; match the predicted straight line feature point with a straight line feature point in the target straight line candidate point set, and take a straight line feature point that is matched successfully as a target straight line feature point; and generate a straight line road marker factor according to the target straight line feature point.

[0129] On the basis of the above-mentioned embodiments, the straight line road marker factor is: , wherein, represents the state at the moment, represents the predicted straight line feature, represents the transformation, represents the observed straight line feature, represents a coordinate system transformation operator under the straight line feature, represents a residual error operator under the straight line feature.

[0130] On the basis of the above-mentioned embodiments, the second generation module 303 is specifically configured to: divide the point cloud data into a plurality of point cloud clusters; calculate the curvatures of the point cloud data in each point cloud cluster; take the point cloud data with the minimum curvature in each point cloud cluster as a plane candidate point; wherein the plurality of plane candidate points constitute a plane candidate point set; and generate a plane road marker factor according to the plane candidate point set.

[0131] On the basis of the above-mentioned embodiments, the second generation module 303 is specifically configured to: obtain a historical plane feature point of a previous frame; predict a predicted plane feature point of a current frame according to the IMU data and the historical plane feature point; perform screening on the plane candidate point set according to the predicted plane feature point, to generate a target plane candidate point set within a preset distance range of the predicted plane feature point; match the predicted plane feature point with a plane feature point in the target plane candidate point set, and take a plane feature point that is matched successfully as a target plane feature point; and generate a plane road marker factor according to the target plane feature point.

[0132] On the basis of the above-mentioned embodiments, the plane road marker factor is: , wherein, represents the state at the moment, Represents the predicted planar features, Indicates transformation, Represents the observed planar features, Operators for coordinate system transformation under planar features. The residual operator represents the operation for calculating residuals under planar features.

[0133] Figure 4 This is a schematic diagram of the electronic device structure provided in the embodiments of this application, such as... Figure 4 As shown, the electronic device includes a processor 401, a memory 402, and a bus 403; wherein the processor 401 and the memory 402 communicate with each other via the bus 403. The processor 401 is used to call program instructions in the memory 402 to execute the methods provided in the above-described method embodiments.

[0134] Processor 401 can be an integrated circuit chip with signal processing capabilities. The processor 401 can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), an off-the-shelf programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. It can implement or execute the various methods, steps, and logic block diagrams disclosed in the embodiments of this application. The general-purpose processor can be a microprocessor or any conventional processor.

[0135] The memory 402 may include, but is not limited to, random access memory (RAM), read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), etc.

[0136] The embodiment discloses a computer program product, the computer program product comprises a computer program stored on a non-transitory computer readable storage medium, the computer program comprises program instructions, when the program instructions are executed by a computer, the computer can execute the method provided by each method embodiment, for example, comprising: acquiring point cloud data of a laser radar, image data of a binocular camera and IMU data of an inertial measurement unit at the same time; generating a visual landmark factor according to the image data; generating a straight line landmark factor and a plane landmark factor according to the point cloud data; generating an IMU pre-integration factor according to the IMU data; constructing a factor graph according to the visual landmark factor, the straight line landmark factor, the plane landmark factor and the IMU pre-integration factor; the factor graph comprises a factor weight; the factor weight is optimized by using reinforcement learning, and an optimized target factor graph is generated; and navigation is performed according to the target factor graph.

[0137] The embodiment provides a non-transitory computer readable storage medium, the non-transitory computer readable storage medium stores computer instructions, the computer instructions enable the computer to execute the method provided by each method embodiment, for example, comprising: acquiring point cloud data of a laser radar, image data of a binocular camera and IMU data of an inertial measurement unit at the same time; generating a visual landmark factor according to the image data; generating a straight line landmark factor and a plane landmark factor according to the point cloud data; generating an IMU pre-integration factor according to the IMU data; constructing a factor graph according to the visual landmark factor, the straight line landmark factor, the plane landmark factor and the IMU pre-integration factor; the factor graph comprises a factor weight; the factor weight is optimized by using reinforcement learning, and an optimized target factor graph is generated; and navigation is performed according to the target factor graph.

[0138] In the embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other manners. The above described device embodiments are merely illustrative. For example, the division of the units is only a logical function division, and there can be another division manner in actual implementation. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed coupling or direct coupling or communication connection between the units can be indirect coupling or communication connection through some communication interfaces, and can be electrical, mechanical or other forms.

[0139] In addition, the units described as separate components can or can not be physically separate, and the components displayed as units can or can not be physical units, that is, they can be located in one place, or can be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiment.

[0140] Further, each functional module in each embodiment of the present application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0141] The above merely provides an example of the present application and is not intended to limit the protection scope of the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A reinforcement learning based multi-sensor fusion navigation method, characterized in that, The method comprises: acquiring point cloud data of a laser radar, image data of a binocular camera and IMU data of an inertial measurement unit at the same time; generating a visual landmark factor according to the image data; generating a straight line landmark factor and a plane landmark factor according to the point cloud data; generating an IMU pre-integration factor according to the IMU data; constructing a factor graph according to the visual landmark factor, the straight line landmark factor, the plane landmark factor and the IMU pre-integration factor; the factor graph comprises a factor weight; optimizing the factor weight by using reinforcement learning to generate an optimized target factor graph; navigating according to the target factor graph; the straight line landmark factor is: wherein, denotes denotes denotes a predicted straight line feature, denotes a transformation, denotes an observed straight line feature, denotes a coordinate system transformation operator under a straight line feature, denotes a residual operator under a straight line feature; the plane landmark factor is: wherein, denotes denotes a state at a time instant, denotes a predicted plane feature, denotes a transformation, denotes an observed plane feature, denotes a coordinate system transformation operator under a plane feature, denotes a residual finding operator under a plane feature.

2. The method of claim 1, wherein, the generating a straight line landmark factor according to the point cloud data comprises: segmenting the point cloud data into a plurality of point cloud clusters; calculating the curvature of each point cloud data in each point cloud cluster; taking the point cloud data with the maximum curvature in each point cloud cluster as a straight line candidate point; wherein a plurality of the straight line candidate points constitute a straight line candidate point set; generating a straight line landmark factor according to the straight line candidate point set.

3. The method of claim 2, wherein, the generating a straight line landmark factor according to the straight line candidate point set comprises: acquiring a historical straight line feature point of a previous frame; predicting a predicted straight line feature point of a current frame according to the IMU data and the historical straight line feature point; screening the straight line candidate point set according to the predicted straight line feature point to generate a target straight line candidate point set within a preset distance range of the predicted straight line feature point; matching the predicted straight line feature point with a straight line feature point in the target straight line candidate point set, and taking the straight line feature point that matches successfully as a target straight line feature point; generating a straight line landmark factor according to the target straight line feature point.

4. The method of claim 1, wherein, the generating a plane landmark factor according to the point cloud data comprises: segmenting the point cloud data into a plurality of point cloud clusters; calculating the curvature of each point cloud data in each point cloud cluster; taking the point cloud data with the minimum curvature in each point cloud cluster as a plane candidate point; wherein a plurality of the plane candidate points constitute a plane candidate point set; generating a plane landmark factor according to the plane candidate point set.

5. The method of claim 4, wherein, the generating a plane landmark factor according to the plane candidate point set comprises: acquiring a historical plane feature point of a previous frame; predicting a predicted plane feature point of a current frame according to the IMU data and the historical plane feature point; screening the plane candidate point set according to the predicted plane feature point to generate a target plane candidate point set within a preset distance range of the predicted plane feature point; matching the predicted plane feature point with a plane feature point in the target plane candidate point set, and taking the plane feature point that matches successfully as a target plane feature point; generating a plane landmark factor according to the target plane feature point.

6. A reinforcement learning based multi-sensor fusion navigation device, characterized by, The device comprises: an acquisition module for acquiring point cloud data of a laser radar, image data of a binocular camera and IMU data of an inertial measurement unit at the same time; a first generation module for generating a visual landmark factor according to the image data; a second generation module for generating a straight line landmark factor and a plane landmark factor according to the point cloud data; a third generation module configured to generate an IMU pre-integration factor according to the IMU data; a construction module configured to construct a factor graph according to the visual landmark factor, the line landmark factor, the plane landmark factor, and the IMU pre-integration factor, wherein the factor graph comprises factor weights; an optimization module configured to optimize the factor weights by reinforcement learning to generate an optimized target factor graph; a navigation module configured to navigate according to the target factor graph; the line landmark factor is wherein, represents represents a state at a time instant, represents a predicted straight line feature, represents a transformation, represents an observed straight line feature, represents a coordinate system transformation operator under a straight line feature, represents a residual operator under a straight line feature; the plane landmark factor is wherein, denotes denotes a state at a time instant, denotes a predicted plane feature, denotes a transformation, denotes an observed plane feature, denotes a coordinate system transformation operator under a plane feature, denotes a residual finding operator under a plane feature.

7. An electronic device, comprising: comprise: a processor and a memory, wherein the memory stores machine readable instructions executable by the processor, and the machine readable instructions, when executed by the processor, perform the method of any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, and the computer program, when executed by a processor, performs the method of any one of claims 1 to 5.

Images