A closed-loop detection method for robot trajectory based on its own motion
By constructing noise models and attention modules, we predict the correlation characteristics of the motion posture and use only the own motion information for closed-loop detection, solving the problem of increasing the cost of visual information matching and achieving efficient closed-loop detection effect.
Patent Information
- Application Number
- CN202310900617.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-07-21
- Publication Date
- 2025-09-02
- Estimated Expiration
- 2043-07-21
AI Technical Summary
In existing closed-loop detection algorithms, in large-scale or long-running SLAM systems, the matching cost of visual or point cloud information will increase rapidly over time, resulting in inefficient detection and unstable changes in view angle and brightness.
The closed-loop detection method of robot trajectory based on its own motion is adopted. By constructing a noise model, denoising attention module, forward and reverse attention module, the correlation characteristics of the motion posture are predicted, and the feature and matching cost function are constructed, the closed-loop detection network model is trained, and closed-loop detection is only used for closed-loop detection.
Under the condition of no visual information, closed-loop location can be effectively detected, which improves detection efficiency and robustness, simulates the grid-like cell distribution mode in the mammalian brain, and achieves efficient closed-loop detection.
Smart Images
Figure CN116922382B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of deep learning and robot synchronous positioning and map construction, and specifically relates to a robot trajectory closed-loop detection method based on its own motion. Background Art
[0002] Loop closure detection is a key component of simultaneous localization and mapping (SLAM) systems. This task typically uses visual information to identify whether a location or scene has already been visited by a robot. This technique is often used to address the accumulated errors in the localization function of SLAM systems over time.
[0003] Most current loop detection algorithms use sensors such as cameras or lidar to capture visual or point cloud information and use this to determine the presence of loop closures. Early visual SLAM methods used a one-dimensional vector created by summing the pixels in each column as an image descriptor, and used this to compare the similarity between the current frame and the past frame to determine loop closures. However, this method is not robust to changes in perspective and brightness. Currently, a popular algorithm is the bag-of-words-based model, which measures the distance of each frame based on pre-trained visual descriptors and quickly queries them through a tree structure. This method has improved the speed and robustness of loop detection in many SLAM systems.
[0004] However, in large-scale or long-running SLAM systems, the cost of matching visual or point cloud information can grow rapidly over time. Therefore, introducing self-motion information helps to quickly eliminate those impossible loop closures, thereby greatly improving the efficiency of loop closure detection. At the same time, motion information can also assist vision-based loop closure detection to improve its performance. Summary of the Invention
[0005] In order to solve the above technical problems, the present invention provides a robot trajectory closed-loop detection method based on its own motion, which can better detect the closed-loop position even without visual information.
[0006] The technical solution adopted by the present invention is: a closed-loop detection method of robot trajectory based on its own motion, the specific steps are as follows:
[0007] S1. Build a noise model and convert the real motion trajectory into a motion trajectory with noise;
[0008] S2, build a denoising attention module and predict the grid feature representation of the noisy motion trajectory;
[0009] S3, construct forward and backward attention modules to capture the correlation features of motion postures, and predict the closed-loop matching probability of motion postures through the correlation features;
[0010] S4, constructing a feature cost function by constraining the position and orientation of grid features;
[0011] S5. Constructing a matching cost function by calculating the similarity of motion postures;
[0012] S6. Combine the features and matching cost function to train the closed-loop detection network model to complete the closed-loop detection of the trajectory.
[0013] Furthermore, the step S1 is specifically as follows:
[0014] First, a noise model is constructed, given a real motion trajectory. The data set of the real motion trajectory Normalized to the range [0,1].
[0015] Trajectory Input noise model, according to the trajectory Calculate the relative pose of any t-th frame and t+1-th frame, and express it as the rotation component θ and the translation component T; the rotation component θ and the translation component T are each superimposed with a noise generated with probability p and obeying the Gaussian distribution or a noise generated with probability 1-p and obeying the uniform distribution. After superimposing the noise, a smooth trajectory is obtained by Gaussian filtering and expressed as the rotation component and translational components
[0016] Calculate the average speed in the x and y directions of the translation component of the real motion trajectory, and weight the translation components of the smooth trajectory in the x and y directions with the average speed to obtain the weighted translation components Then calculate the module of the average velocity in the x and y directions of the translation component of the real motion trajectory, and the rotation component of the smooth trajectory Perform weighting to obtain the weighted rotation component The details are as follows:
[0017]
[0018]
[0019] Finally, the weighted translation and rotation components are combined into a noisy motion trajectory
[0020] Furthermore, the step 2 is specifically as follows:
[0021] Using standard fully connected layers, embedding layers, and multi-head attention modules, a denoising attention module is built. This module accepts input of a noisy motion trajectory and predicts a grid-like feature representation of the trajectory F. gc .
[0022] Given a noisy motion trajectory Calculate the temporal feature F using the temporal order of the trajectory time , which is calculated by a standard embedding layer; the velocity feature F is calculated using the velocity of the trajectory vel , which is calculated by a labeled fully connected layer; the rotation component of the trajectory is used Calculate the angle feature F hd , which is calculated by simulating the firing patterns of head-facing cells in the mammalian brain as follows:
[0023]
[0024] Among them, μ θ and k represent pre-set parameters, and softmax represents a labeled softmax function.
[0025] Use the translation component of the trajectory Calculate the position feature F pc , which is calculated by simulating the firing patterns of place cells in the mammalian brain as follows:
[0026]
[0027] Among them, μ T and σ T represents a pre-set parameter, softmax represents a labeled softmax function, and |·|2 represents the 2-norm of the vector.
[0028] Then the time feature F time , speed characteristic F vel , position feature F pc and angle feature F hd Sum and pass it into a standard multi-head attention module to predict the grid feature F of the noisy motion trajectory gc .
[0029] Furthermore, the step S3 is specifically as follows:
[0030] S31. Construct a forward attention module that focuses only on the predecessor point and a backward attention module that focuses only on the successor point;
[0031] Construct a lower triangular matrix with a value of 1 and multiply it by the attention calculation part in the standard multi-head attention module to obtain the forward attention module; construct an upper triangular matrix with a value of 1 and multiply it by the attention calculation part in the standard multi-head attention module to obtain the reverse attention module.
[0032] S32. Input grid feature representation F gc, forward attention features and reverse attention features are obtained through the forward attention module and reverse attention module respectively;
[0033] Grid feature F gc Pass in a forward attention module to calculate the forward attention features Grid feature F gc Pass in a reverse attention module to calculate the reverse attention feature
[0034] S33, predicting the similarity between each frame in the trajectory and other frames, and obtaining the probability of whether the frame is a closed-loop position based on the similarity;
[0035] By forward attention feature and reverse attention features Calculate the similarity S between each frame and other frames in the trajectory:
[0036]
[0037] Here, sig represents the standard sigmoid function.
[0038] Closed-loop matching probability P lc is the average of the similarity matrix S along the second dimension.
[0039] When the closed-loop matching probability P lc When it is >0.5, the point is considered to be a closed loop point.
[0040] Furthermore, the step S4 is specifically as follows:
[0041] According to the grid feature representation F calculated in step S2 gc , calculate the position and angle encoding of each frame feature as the feature cost function;
[0042] First, calculate the grid feature representation F gc The predicted position P pc :
[0043] P pc =softmax(FC(F gc ))
[0044] Among them, FC represents the standard fully connected layer.
[0045] Then calculate the grid feature representation F gc The predicted angle P hd :
[0046] P hd =softmax(FC(F gc ))
[0047] Given multiple preset positions PC, the position cost function L pc The expression is as follows:
[0048]
[0049] Among them, PC i represents the position of the i-th preset, represents the position of the i-th prediction, |·|1 represents the 1-norm of the vector, and N represents the number of samples in each training batch.
[0050] Given multiple preset angles HD, the angle cost function L hd The expression is as follows:
[0051]
[0052] Among them, HD i represents the i-th preset angle, represents the angle of the i-th prediction.
[0053] Finally, the position cost function L pc And the angle cost function L hd The sum is obtained to obtain the feature cost function.
[0054] Furthermore, the step S5 is specifically as follows:
[0055] Calculate the true motion trajectory The distance similarity is the exponential function of the negative error between the position at any time t and the position at all other times in the trajectory. The calculation formula for the position similarity between the i-th time and the j-th time in the trajectory is:
[0056]
[0057] in, and represents the i-th and j-th moments in the trajectory, |·|2 represents the 2-norm of the vector, and σ represents a predefined parameter.
[0058] Calculate the standard cross entropy of the distance similarity between the predicted similarity S and the true trajectory in step S3 and use it as the matching cost function.
[0059] Furthermore, the step S6 is specifically as follows:
[0060] Based on steps S2 and S3, a closed-loop detection network model is constructed and the noisy trajectory in step S1 is input. Output the grid feature representation F in step S2 gcThe probability of whether it is a closed-loop position in step S3 is summed up through the feature cost function and matching cost function constructed in steps S4 and S5 to obtain the total cost function and train the closed-loop detection network model. That is, the gradient of each parameter in the model of steps S2-S3 is calculated using the general standard back propagation algorithm, and each parameter is updated through the standard Adam algorithm and repeated M times to complete the closed-loop detection of the trajectory.
[0061] Where M∈[5000,20000].
[0062] Furthermore, the detection method further includes step S7, after the closed-loop detection network model is trained, any estimated self-motion trajectory is input, and whether it is a closed-loop position is calculated through steps S2-S3.
[0063] The beneficial effects of the present invention are as follows: the method of the present invention constructs a noise model using noisy motion trajectories and real motion trajectories, constructs a denoising attention module to predict the grid-like features of the noisy motion trajectory, then constructs forward and backward attention modules to capture the correlation features of the motion posture, predicts the closed-loop matching probability of the motion posture through the correlation features, constrains the position and direction of the grid-like features to construct a feature cost function, calculates the similarity of the motion posture to construct a matching cost function, and finally combines the features and matching cost function to train a model to complete the closed-loop detection of the trajectory. The method of the present invention is inspired by the memory and navigation system of mammals to design a grid-like feature representation cost function, predicts the grid-like features of the noisy motion trajectory, and estimates its own closed-loop probability using only its own motion information. In the absence of visual information, it achieves better closed-loop detection results for the robot motion trajectory and can better detect the closed-loop position. BRIEF DESCRIPTION OF THE DRAWINGS
[0064] Figure 1 This is a flow chart of a closed-loop detection method for robot trajectory based on its own motion according to the present invention.
[0065] Figure 2 Schematic diagram of the training and testing phases of the method of the present invention in an embodiment of the present invention.
[0066] Figure 3 3 is a comparison chart of closed-loop detection prediction results between the method of the present invention and the existing method in an embodiment of the present invention. DETAILED DESCRIPTION
[0067] The method of the present invention is further described below with reference to the accompanying drawings and examples.
[0068] like Figure 1 As shown in the flowchart of a closed-loop detection method of robot trajectory based on its own motion of the present invention, the specific steps are as follows:
[0069] S1. Build a noise model and convert the real motion trajectory into a motion trajectory with noise;
[0070] S2, build a denoising attention module and predict the grid feature representation of the noisy motion trajectory;
[0071] S3, construct forward and backward attention modules to capture the correlation features of motion postures, and predict the closed-loop matching probability of motion postures through the correlation features;
[0072] S4, constructing a feature cost function by constraining the position and orientation of grid features;
[0073] S5. Constructing a matching cost function by calculating the similarity of motion postures;
[0074] S6. Combine the features and matching cost function to train the closed-loop detection network model to complete the closed-loop detection of the trajectory.
[0075] In this embodiment, step S1 is specifically as follows:
[0076] First, a noise model is constructed, given a real motion trajectory. The data set of the real motion trajectory Normalized to the range [0,1].
[0077] Trajectory Input noise model, according to the trajectory Calculate the relative pose of any t-th frame and t+1-th frame, and express it as the rotation component θ and the translation component T; the rotation component θ and the translation component T are each superimposed with a noise generated with probability p and obeying the Gaussian distribution or a noise generated with probability 1-p and obeying the uniform distribution. After superimposing the noise, a smooth trajectory is obtained by Gaussian filtering and expressed as the rotation component and translational components
[0078] Calculate the average speed in the x and y directions of the translation component of the real motion trajectory, and weight the translation components of the smooth trajectory in the x and y directions with the average speed to obtain the weighted translation components Then calculate the module of the average velocity in the x and y directions of the translation component of the real motion trajectory, and the rotation component of the smooth trajectory Perform weighting to obtain the weighted rotation component The details are as follows:
[0079]
[0080]
[0081] Finally, the weighted translation and rotation components are combined into a noisy motion trajectory The noise is intended to simulate the ego motion trajectory estimated by the odometry method in real situations.
[0082] In this embodiment, step S2 is specifically as follows:
[0083] Using standard fully connected layers, embedding layers, and multi-head attention modules, a denoising attention module is built. This module accepts input of a noisy motion trajectory and predicts a grid-like feature representation of the trajectory F. gc ;
[0084] Given a noisy motion trajectory Calculate the temporal feature F using the temporal order of the trajectory time , which is calculated by a standard embedding layer; the velocity feature F is calculated using the velocity of the trajectory vel , which is calculated by a labeled fully connected layer; the rotation component of the trajectory is used Calculate the angle feature F hd , which is calculated by simulating the firing patterns of head-facing cells in the mammalian brain as follows:
[0085]
[0086] Among them, μ θ and k represent pre-set parameters, and softmax represents a labeled softmax function.
[0087] Use the translation component of the trajectory Calculate the position feature F pc , which is calculated by simulating the firing patterns of place cells in the mammalian brain as follows:
[0088]
[0089] Among them, μ T and σ T represents a pre-set parameter, softmax represents a labeled softmax function, and |·|2 represents the 2-norm of the vector.
[0090] Then the time feature F time , speed characteristic F vel , position feature F pc and angle feature F hd Sum and pass it into a standard multi-head attention module to predict the grid feature F of the noisy motion trajectory gc .
[0091] In this embodiment, step S3 is specifically as follows:
[0092] The step S3 is specifically as follows:
[0093] S31. Construct a forward attention module that focuses only on the predecessor point and a backward attention module that focuses only on the successor point;
[0094] Construct a lower triangular matrix with a value of 1 and multiply it by the attention calculation part in the standard multi-head attention module to obtain the forward attention module; construct an upper triangular matrix with a value of 1 and multiply it by the attention calculation part in the standard multi-head attention module to obtain the reverse attention module.
[0095] S32. Input grid feature representation F gc , forward attention features and reverse attention features are obtained through the forward attention module and reverse attention module respectively;
[0096] Grid feature F gc Pass in a forward attention module to calculate the forward attention features Grid feature F gc Pass in a reverse attention module to calculate the reverse attention feature
[0097] S33, predicting the similarity between each frame in the trajectory and other frames, and obtaining the probability of whether the frame is a closed-loop position based on the similarity;
[0098] By forward attention feature and reverse attention features Calculate the similarity S between each frame and other frames in the trajectory:
[0099]
[0100] Here, sig represents the standard sigmoid function.
[0101] Closed-loop matching probability P lc is the average of the similarity matrix S along the second dimension.
[0102] When the closed-loop matching probability P lc When it is >0.5, the point is considered to be a closed loop point.
[0103] In this embodiment, step S4 is specifically as follows:
[0104] According to the grid feature representation F calculated in step S2 gc , calculate the position and angle encoding of each frame feature as the feature cost function;
[0105] First, calculate the grid feature representation F gc The predicted position P pc :
[0106] P pc=softmax(FC(F gc ))
[0107] Among them, FC represents the standard fully connected layer.
[0108] Then calculate the grid feature representation F gc The predicted angle P hd :
[0109] P hd =softmax(FC(F gc ))
[0110] Given multiple preset positions PC, the position cost function L pc The expression is as follows:
[0111]
[0112] Among them, PC i represents the position of the i-th preset, represents the position of the i-th prediction, |·|1 represents the 1-norm of the vector, and N represents the number of samples in each training batch.
[0113] Given multiple preset angles HD, the angle cost function L hd The expression is as follows:
[0114]
[0115] Among them, HD i represents the i-th preset angle, represents the angle of the i-th prediction.
[0116] Finally, the position cost function L pc And the angle cost function L hd The sum is obtained to obtain the feature cost function.
[0117] In an embodiment, step S5 is specifically as follows:
[0118] Calculate the true motion trajectory The distance similarity is the exponential function of the negative error between the position at any time t and the position at all other times in the trajectory. The calculation formula for the position similarity between the i-th time and the j-th time in the trajectory is:
[0119]
[0120] in, and represents the i-th and j-th moments in the trajectory, |·|2 represents the 2-norm of the vector, and σ represents a predefined parameter.
[0121] Calculate the standard cross entropy of the distance similarity between the predicted similarity S and the true trajectory in step S3 and use it as the matching cost function.
[0122] In this embodiment, step S6 is specifically as follows:
[0123] Based on steps S2 and S3, a closed-loop detection network model is constructed and the noisy trajectory in step S1 is input. Output the grid feature representation F in step S2 gc The probability of whether it is a closed-loop position in step S3 is summed up through the feature cost function and matching cost function constructed in steps S4 and S5 to obtain the total cost function and train the closed-loop detection network model. That is, the gradient of each parameter in the model of steps S2-S3 is calculated using the general standard back propagation algorithm, and each parameter is updated through the standard Adam algorithm and repeated M times to complete the closed-loop detection of the trajectory.
[0124] Wherein, M∈[5000,20000], in this embodiment, M=10000.
[0125] like Figure 2 As shown, in this embodiment, the detection method further includes step S7, after the closed-loop detection network model is trained, any estimated self-motion trajectory is input, and whether it is a closed-loop position is calculated through steps S2-S3.
[0126] like Figure 3 As shown in this example, the method of the present invention uses existing monocular ORB-SLAM algorithms, binocular ORB-SLAM algorithms, and monocular GVO algorithms as inputs, and conducts experiments and analysis on the KITTI dataset. The light-colored areas in the figure represent estimated closed loops. The experimental results demonstrate that the method of the present invention can achieve good results even when relying solely on trajectory information. Furthermore, the present invention can learn firing patterns similar to those of grid cells in the mammalian brain.
[0127] In summary, the method of the present invention is inspired by the memory and navigation system of mammals to design a grid-like feature representation cost function, predict the grid-like features of the noisy motion trajectory, estimate its own closed-loop probability using only its own motion information, and obtain better closed-loop detection results for the robot motion trajectory in the absence of visual information, and can better detect the closed-loop position.
[0128] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and it should be understood that the scope of protection of the present invention is not limited to such specific descriptions and embodiments. Those skilled in the art can make various other specific variations and combinations based on the technical teachings disclosed in the present invention without departing from the essence of the present invention, and such variations and combinations are still within the scope of protection of the present invention.
Claims
1. A closed-loop detection method for robot trajectory based on its own motion, the specific steps are as follows: S1. Build a noise model and convert the real motion trajectory into a motion trajectory with noise; The real motion trajectory Input the noise model, calculate its relative pose between any t-th frame and t+1-th frame, and express it as the rotation component θ and the translation component T; the rotation component θ and the translation component T are superimposed with a noise and then Gaussian filtered to obtain a smooth trajectory, which is expressed as the rotation component and translational components The rotation component and translational components The weighted combination is a noisy motion trajectory S2, build a denoising attention module and predict the grid feature representation of the noisy motion trajectory; Use the standard fully connected layer, embedding layer and multi-head attention module to build a denoising attention module, and then use the temporal feature F time , speed characteristic F vel , position feature F pc and angle feature F hd Sum and pass it into a standard multi-head attention module to predict the grid feature F of the noisy motion trajectory gc ; S3. Construct a forward attention module and a reverse attention module to capture the correlation features of motion postures, and predict the closed-loop matching probability of motion postures based on the correlation features of motion postures; First, we construct a forward attention module that focuses only on the predecessor point and a reverse attention module that focuses only on the successor point, and then input the grid feature representation F gc , respectively, obtain the forward attention feature and the reverse attention feature through the forward attention module and the reverse attention module, and finally predict the similarity between each frame in the trajectory and other frames, and obtain the probability of whether the frame is a closed-loop position based on the similarity; S4, constructing a feature cost function by constraining the position and orientation of grid features; According to the grid feature representation F calculated in step S2 gc , calculate the position cost function and the angle cost function, and finally, sum the position cost function and the angle cost function to obtain the feature cost function; S5. Constructing a matching cost function by calculating the similarity of motion postures; Calculate the true motion trajectory The distance similarity of the predicted trajectory S is calculated, and the standard cross entropy of the distance similarity between the predicted trajectory S and the true trajectory is calculated as the matching cost function; S6. Combine the feature cost function and the matching cost function to train the closed-loop detection network model and complete the closed-loop detection of the trajectory; The feature cost function and the matching cost function are summed to obtain the total cost function and train the closed-loop detection network model.
2. A closed-loop detection method for robot trajectory based on self-motion according to claim 1, characterized in that: The step S1 is specifically as follows: First, a noise model is constructed, given a real motion trajectory. The data set of the real motion trajectory Normalized to the range [0,1]; Trajectory Input noise model, according to the trajectory Calculate the relative pose of any t-th frame and t+1-th frame, and express it as the rotation component θ and the translation component T; the rotation component θ and the translation component T are each superimposed with a noise generated with probability p and obeying the Gaussian distribution or a noise generated with probability 1-p and obeying the uniform distribution. After superimposing the noise, a smooth trajectory is obtained by Gaussian filtering and expressed as the rotation component and translational components Calculate the average speed in the x and y directions of the translation component of the real motion trajectory, and weight the translation components of the smooth trajectory in the x and y directions with the average speed in the x and y directions of the translation component of the real motion trajectory to obtain the weighted translation component Then calculate the module of the average velocity in the x and y directions of the translation component of the real motion trajectory, and the rotation component of the smooth trajectory Perform weighting to obtain the weighted rotation component The details are as follows: Finally, the weighted translation and rotation components are combined into a noisy motion trajectory 3. The closed-loop detection method of a robot trajectory based on its own motion according to claim 1, characterized in that: The step 2 is specifically as follows: Use standard fully connected layers, embedding layers, and multi-head attention modules to build a denoising attention module; The denoising attention module accepts the input of the noisy motion trajectory and predicts the grid feature representation F of the trajectory. gc ; Given a noisy motion trajectory Calculate the temporal feature F using the temporal order of the trajectory time , the time feature is calculated by a standard embedding layer; the velocity feature F is calculated using the velocity of the trajectory vel , the velocity feature is calculated by a labeled fully connected layer; the rotation component of the trajectory is used Calculate the angle feature F hd , the angular features are calculated by simulating the firing patterns of head heading cells in the mammalian brain as follows: Among them, μ θ and k represent pre-set parameters, and softmax represents a labeled softmax function; Use the translation component of the trajectory Calculate the position feature F pc , the position features are calculated by simulating the firing patterns of place cells in the mammalian brain as follows: Among them, μ T and σ T represents a pre-set parameter, softmax represents a labeled softmax function, and |·|2 represents the 2-norm of the vector; Then the time feature F time , speed characteristic F vel , position feature F pc and angle feature F hd Sum and pass it into a standard multi-head attention module to predict the grid feature F of the noisy motion trajectory gc .
4. The closed-loop detection method of a robot trajectory based on its own motion according to claim 1, characterized in that: The step S3 is specifically as follows: S31. Construct a forward attention module that focuses only on the predecessor point and a backward attention module that focuses only on the successor point; Construct a lower triangular matrix with a value of 1 and multiply it by the attention calculation part in the standard multi-head attention module to obtain the forward attention module; construct an upper triangular matrix with a value of 1 and multiply it by the attention calculation part in the standard multi-head attention module to obtain the reverse attention module; S32. Input grid feature representation F gc , forward attention features and reverse attention features are obtained through the forward attention module and reverse attention module respectively; Grid feature F gc Pass in a forward attention module to calculate the forward attention features Grid feature F gc Pass in a reverse attention module to calculate the reverse attention feature S33, predicting the similarity between each frame in the trajectory and other frames, and obtaining the probability of whether the frame is a closed-loop position based on the similarity; By forward attention feature and reverse attention features Calculate the similarity S between each frame and other frames in the trajectory: Among them, sig represents the standard sigmoid function; Closed-loop matching probability P lc is the average of the similarity matrix S along the second dimension; When the closed-loop matching probability P lc When it is >0.5, the point is considered to be a closed loop point.
5. The closed-loop detection method of a robot trajectory based on its own motion according to claim 1, characterized in that: The step S4 is specifically as follows: According to the grid feature representation F calculated in step S2 gc , calculate the position and angle encoding of each frame feature as the feature cost function; First, calculate the grid feature representation F gc The predicted position P pc : P pc =softmax(FC(F gc )) Among them, FC represents the standard fully connected layer; Then calculate the grid feature representation F gc The predicted angle P hd : P hd =softmax(FC(F gc )) Given multiple preset positions PC, the position cost function L pc The expression is as follows: Among them, PC i represents the position of the i-th preset, represents the position of the i-th prediction, |·|1 represents the 1-norm of the vector, and N represents the number of samples in each training batch; Given multiple preset angles HD, the angle cost function L hd The expression is as follows: Among them, HD i represents the i-th preset angle, represents the angle of the i-th prediction; Finally, the position cost function L pc And the angle cost function L hd The sum is obtained to obtain the feature cost function.
6. The closed-loop detection method of a robot trajectory based on its own motion according to claim 1, characterized in that: The step S5 is specifically as follows: Calculate the true motion trajectory The distance similarity is the exponential function of the negative error between the position at any time t and the position at all other times in the trajectory. The calculation formula for the position similarity between the i-th time and the j-th time in the trajectory is: in, and represents the i-th and j-th moments in the trajectory, |·|2 represents the 2-norm of the vector, and σ represents a predefined parameter; Calculate the standard cross entropy of the distance similarity between the predicted similarity S and the true trajectory in step S3 and use it as the matching cost function.
7. The closed-loop detection method of a robot trajectory based on its own motion according to claim 1, characterized in that: The step S6 is specifically as follows: Based on steps S2 and S3, a closed-loop detection network model is constructed and the noisy trajectory in step S1 is input. Output the grid feature representation F in step S2 gc The probability of whether it is a closed-loop position in step S3 is summed through the feature cost function and matching cost function constructed in steps S4 and S5 to obtain the total cost function and train the closed-loop detection network model. That is, the gradient of each parameter in the model of steps S2-S3 is calculated using the general standard back-propagation algorithm, and each parameter is updated using the standard Adam algorithm and repeated M times to complete the closed-loop detection of the trajectory; Where M∈[5000,20000].
8. The closed-loop detection method of a robot trajectory based on its own motion according to claim 1, characterized in that: The detection method further includes step S7, which is specifically as follows: After the closed-loop detection network model is trained, any estimated self-motion trajectory is input and whether it is a closed-loop position is calculated through steps S2-S3.