Multi-unmanned aerial vehicle cooperation SLAM system based on common-view area detection

Abstract

The invention discloses a multi-unmanned-aerial-vehicle cooperative SLAM system based on common-view area detection. According to the system, firstly, an adaptive threshold value and a feature homogenization method are introduced to improve a feature extraction algorithm to extract image features; secondly, on the basis of a feature point screening algorithm fusing YOLO and geometric constraints, the influence of motion exterior points is reduced; then, an improved feature point screening mechanism is provided for the problem of feature mismatching existing in the unmanned aerial vehicle image; thirdly, on the basis of local positioning and mapping of the unmanned aerial vehicle, a common-view area detection algorithm based on a visual word bag and relative pose calculation are fused into a local map; and finally, performing global optimization by adopting an improved L-M solution algorithm to improve pose estimation, and generating a high-precision global map. According to the method, the multi-unmanned aerial vehicle cooperative SLAM system is improved, so that the interference of movement obstacles in an unknown environment is reduced, the mapping and positioning precision is improved, and better real-time performance and robustness are achieved.

Description

Technical Field

[0001] The present invention belongs to the field of flight control, and in particular relates to a multi-UAV cooperative SLAM system based on common view area detection. Technical Background

[0002] UAV visual SLAM technology is an important research field for autonomous navigation of UAVs. Collaborative SLAM can integrate data from multiple UAVs to achieve information sharing and task allocation, effectively improving the accuracy, efficiency and robustness of the system. Compared with the problems of narrow observation range, limited computing power, low execution efficiency and weak anti-interference ability of a single UAV system, collaborative SLAM can integrate data from multiple machines and effectively improve the accuracy, efficiency and robustness of the system through information sharing and task allocation. In recent years, collaborative SLAM systems have been widely used in disaster relief, agriculture, geographic surveying and mapping, virtual reality and other fields. For example, in disaster relief, a flexible collaborative system can greatly shorten the time to generate a global map, winning valuable time for search personnel.

[0003] However, the increasingly complex unknown environment and mission requirements also put forward higher requirements on the environmental adaptability and autonomous navigation of UAVs. Collaborative SLAM requires an effective system architecture to ensure data consistency in the entire SLAM system, and is responsible for actual data sharing and effective resource allocation. Schmuck et al. proposed the COVINS collaborative SLAM framework. In the system, each client only retains the basic visual odometer and shares the local map with the server. However, due to the ORB-SLAM3 system adopted by the client, the computing resources are consumed and it is difficult to meet the real-time requirements. The RAMA-SLAM proposed by Liu et al. takes into account the resource competition situation. By decoupling the closed-loop detection on the server side and the VIO module on the client side, the computational complexity and complexity of the collaborative positioning algorithm are effectively reduced. However, the above collaborative SLAM work often ignores the dynamic and complex real scenes. There are often many moving objects in real scenes. Traditional visual SLAM systems usually extract invariant features in the scene, such as corners, edges, etc., and use them to infer the camera's own posture changes. When there are dynamic objects in the scene, the feature points are likely to be distributed on the dynamic objects and change with the changes of the dynamic objects. At this time, it is difficult for the camera to determine whether it is the scene change or its own posture change. DynaSLAM proposed by Bescos et al. uses a detection method based on the Mask R-CNN model to improve system accuracy, but the model used is based on pixel segmentation, which leads to low efficiency. Wu et al. use another more efficient YOLO target detection model to make up for the lack of low efficiency to a certain extent, but it cannot handle potential moving objects. In addition, collaborative SLAM also needs to consider practical issues such as bandwidth, communication delay and packet loss in the actual environment. Summary of the invention

[0004] The purpose of the present invention is to provide a multi-UAV cooperative SLAM system based on common view area detection, so as to improve the robustness of the SLAM system in a dynamic environment, reduce the consumption of client computing resources, and improve real-time performance.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions:

[0006] A multi-UAV cooperative SLAM system based on common view area detection, comprising:

[0007] Step 1: Obtain an image sequence from a visual sensor;

[0008] Step 2: extract representative features from the image sequence using an improved feature extraction algorithm that introduces adaptive threshold and feature homogenization;

[0009] Step 3: Input the image sequence obtained in step 1 into the lightweight YOLO model to predict the target and its category;

[0010] Step 4: Using a dynamic feature point screening algorithm to remove feature points on dynamic objects in the image data to reduce interference from dynamic objects;

[0011] Step 5: According to the grayscale invariance of adjacent image frames and the consistency of neighborhood motion, the image matching points are identified and the outliers are excluded by using the improved RANSAC algorithm;

[0012] Step 6: Convert the two-dimensional pixels observed by the camera into three-dimensional world coordinates through coordinate transformation to construct a local map of the drone;

[0013] Step 7: Each drone is used as a client, and the server receives the local map information of each drone. The local map is fused based on the common view area detection algorithm of the visual word bag and the relative pose calculation;

[0014] Step 8: Use the improved LM solution algorithm to perform global optimization to improve pose estimation and generate a high-precision global map.

[0015] The present invention has the following advantages:

[0016] 1. The present invention extracts semantic information of UAV images by constructing a lightweight target detection network, combines target detection and geometric constraint relationships to filter dynamic objects, and adopts a feature screening mechanism to further reduce the feature mismatch rate, thereby reducing the computational burden and improving the accuracy of the system, making it more suitable for performing positioning and mapping tasks in dynamic environments.

[0017] 2. The present invention can enhance the data association between UAVs based on the common view area detection algorithm, and fuse local maps by calculating the relative postures between UAVs, and use the improved LM solution algorithm for global optimization, thereby improving the accuracy of global maps and posture estimation, and having better real-time and robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 Flow chart of the method of the present invention.

[0020] Figure 2 These are photos taken during the drone flight.

[0021] Figure 3 It is the collaborative flight trajectory of multiple drones.

[0022] Figure 4 Schematic diagram of trajectory error of multi-UAV collaborative SLAM system. DETAILED DESCRIPTION

[0023] The technical solution of the present invention is described in detail with reference to the accompanying drawings.

[0024] like Figure 1 As shown, the present invention provides a multi-UAV cooperative SLAM system based on common view area detection, which specifically includes the following steps:

[0025] Step 1: Obtain a 640×640×3 original image sequence from a visual sensor;

[0026] Step 2 uses an improved feature extraction algorithm that introduces adaptive threshold and feature homogenization to extract representative features from the image sequence, specifically:

[0027] (2.1) In the traditional ORB feature extraction algorithm, FAST is selected as the key point through an adaptive threshold to improve the FAST key point detection speed;

[0028] (2.2) Using BRIEF descriptor composed of binary strings for feature description;

[0029] (2.3) The image pyramid method is used to extract multi-scale features from feature points. The distribution of ORB feature points on the same level of the image is uniformed by quadtree, which improves the problem of uneven distribution of feature points on the same level, ensures that the image has multi-scale rotation and no deformation, and improves the stability of the SLAM system during complex maneuvers.

[0030] Step 3: Input the image sequence obtained in step 1 into the lightweight YOLO model to predict the target and its category, specifically:

[0031] (3.1) In the input part, the original 640×640×3 input image is resized to 416×416×3 to reduce the computational complexity and improve the processing speed;

[0032] (3.2) In the backbone network, MobileNetV3 is used for feature extraction, including ConvModule, Bottle Neck1, Bottle Neck6, Bottle Neck13 and Bottle Neck17. Depthwise separable convolution is used to make YOLOv5s more lightweight, reducing the number of model parameters and computational cost.

[0033] (3.3) In the Neck part, based on the idea of ​​feature pyramid network, the feature maps are integrated through top-down paths and lateral connections to capture three-scale target information.

[0034] (3.4) In the Prediction part, a multi-scale detection strategy is used to generate feature maps of three scales: 13×13, 26×26, and 52×52. By dividing the image into grids corresponding to their sizes, the prediction training results, i.e., the location and category information of the target in the image, are output.

[0035] Step 4 uses a dynamic feature point screening algorithm to remove feature points on dynamic objects in the image to reduce dynamic object interference for the drone. Specifically:

[0036] (4.1) Set the threshold τ as the maximum distance threshold of the static point;

[0037] (4.2) Let the feature point on the previous frame be P 1 , the feature point on the current frame image is P 2 , determine the epipolar line, that is, the projection line of the ray determined by the two points in space on the image plane where the two feature points are located;

[0038] (4.3) Calculate the distance from the feature point to the epipolar line. Suppose the distances from the two feature points to the epipolar line are d and d′ respectively;

[0039] (4.4) If d+d′>τ, then p 1 and p 2 Dynamic feature points in image data are eliminated;

[0040] (4.5) If d+d′<τ, then P 1 and P 2 It is the static feature point in the image data, which is retained;

[0041] (4.6) Update the internal point set to obtain the final correct static feature point pair P′ 1 and P′ 2 .

[0042] Step 5: Based on the grayscale invariance of adjacent image frames and the consistency of neighborhood motion, the image matching points are identified and outliers are excluded by using the improved RANSAC algorithm. Specifically:

[0043] (5.1) Set the maximum number of iterations K m satisfy:

[0044]

[0045] Where η 0 is the confidence level, m is the subset size, and ε is the proportion of internal points in the data set;

[0046] (5.2) Normalize the coordinates of all matching point pairs, divide the original image frame into small boxes of the same size, and calculate the grayscale difference ΔI of all matching point pairs in the same area of ​​adjacent frames. j And the average grayscale difference P(I) = ∑ΔI j , set the average grayscale difference as the threshold;

[0047] (5.3) In the grayscale difference ΔI j In the box less than the set threshold P(I), select the optimal sample points whose Hamming distance and grayscale difference are both less than the threshold P(I) as the initial sample set, and calculate the basic matrix F, which specifically satisfies:

[0048]

[0049] In the formula, A 1 and A 2 They refer to the intrinsic parameter matrices of the front and rear cameras respectively;

[0050] (5.4) According to the grayscale difference ΔI j Expand the sample set in increasing order, calculate the reprojection error of the expanded sample point pairs and filter the internal point set according to the set threshold P(I);

[0051] (5.5) Update the inlier set. If the current inlier scale is larger than the minimum number of inliers N required for the improved RANSAC algorithm, output the inlier set and the basic matrix F, otherwise repeat the previous step.

[0052] Step 6: Convert the two-dimensional pixels observed by the camera into three-dimensional world coordinates through coordinate transformation to construct a local map of the drone. The coordinate transformation formula is:

[0053]

[0054] In the formula, Z c is the coordinate value of the camera, (u, v) is the pixel coordinate, u 0 and v0 O 2 The coordinates in the pixel coordinate system, dU and dV are in meters / pixel, f is the focal length of the camera, R is the rotation matrix, t is the translation vector, (x, y, z) is the camera coordinate, (X, Y, Z) is the world coordinate, and f x =f / dU,f y =f / dV,c x =u 0 , c y =v 0 .

[0055] Step 7: Each drone is used as a client. The server receives the local map information of each drone and fuses the local map based on the common view area detection algorithm of the visual word bag and the relative pose calculation. Specifically:

[0056] (7.1) Receive local map information of each drone;

[0057] (7.2) Improve the common view region detection algorithm based on visual word bag, which mainly includes three aspects: building a visual dictionary, extracting word bag feature vectors from images, and similarity calculation. Specifically:

[0058] (7.2.1) The dictionary generation algorithm based on the K-ary tree structure obtains a tree-structured dictionary, in which each leaf node corresponds to a cluster center, that is, a visual word. When it is necessary to map image features to visual words, search downward from the root node step by step until the most similar cluster center is found;

[0059] (7.2.2) Each feature point corresponds to a visual word in the dictionary, and then the bag-of-words vector of the image frame is generated;

[0060] (7.2.3) An improved similarity calculation method is used to improve the calculation accuracy of the similarity between bag-of-words vectors. The improved similarity calculation formula is:

[0061]

[0062] In the formula, is the bag-of-words vector and The number of words in the union of visual words, coefficient |v a ∪v b |*log(N) is the normalization parameter, word i is the bag-of-words vector and The visual words in the intersection, N is the number of all images in the query dataset, N i Contains the visual word word i The number of visual images.

[0063] (7.3) The local map information of the UAV is updated and optimized through the common view area detection algorithm, and the relative pose transformation between different local maps is calculated. The local map is transformed through the transformation matrix T A，i Fusion into the global map, specifically:

[0064] (7.3.1) UAV A In t i Moment, UAV B In t j Passing the same location M at all times, UAV A and UAVs B The poses in their respective local map coordinate systems are T A，i and T B,j At t j Moment, UAV A The pose in its local map is T A,j , then UAV A From i Time to t j The cumulative posture transformation at the moment is:

[0065] T A，i→j =T A，i+1 T A，i+2 ...T A，j-1 T A,j (5)

[0066] (7.3.2) Using the common location M as the reference point, the UAV A In t j The local map information constructed at time t is converted to i In the coordinate system of the moment, the specific transformation matrix T A，i for:

[0067] T A，i =(T A,i→j ) -1 T A，j (6)

[0068] (7.3.3) The PnP algorithm is used to calculate the initial camera pose estimate based on the known map feature points and their corresponding projection points in the camera coordinate system. The final optimized camera pose is the optimal pose transformation matrix T * ;

[0069] (7.3.4) UAV B The local map is used as the reference system, and the UAV is transformed into A Map point p in the local map A Convert to UAV B The corresponding point p′ in the local map reference systemA , the conversion formula is:

[0070]

[0071] (7.3.5) Convert the map with the common area as the origin obtained in the previous step into a map with the UAV as the origin. B For a map with the initial coordinates as the origin, the conversion formula is:

[0072]

[0073] Where, T B,0 , T B,1 ...T B，j For UAV B From the initial time to t j The pose transformation matrix at the moment.

[0074] (7.3.6) By calculating the relative pose and performing map fusion, a unified global map and accurate pose estimation are obtained, and positioning and mapping are preliminarily completed.

[0075] Step 8: Use the improved LM solution algorithm to perform global optimization to improve pose estimation and generate a high-precision global map. Specifically:

[0076] (8.1) According to the global BA optimization, the reprojection error is minimized to improve the system accuracy. The specific cost function of BA optimization is:

[0077]

[0078] In the formula, z ij is the position ξ i Observation map point p j The data generated;

[0079] (8.2) Define the cost function independent variable x = [ξ 1 , ..., ξ m , p 1 , ..., p n ] T , given an initial value x 0 , and the initial optimization radius μ, when an increment Δx is given to the independent variable x, the function grows to:

[0080]

[0081] In the formula, F ij represents the partial derivative of the cost function with respect to the camera posture in the current state, E ij represents the partial derivative of the function with respect to the position of the landmark point;

[0082] (8.3) In order to improve the solution efficiency, a memory factor is introduced, and the trust region radius λ of the kth iteration is k Set to:

[0083]

[0084] In the formula, μ k is an adjustable parameter, which is adjusted according to the relative decrease of the objective function after each iteration. k is the relative decrease of the cost function after each iteration;

[0085] (8.4) For the kth iteration, solve the following equation:

[0086]

[0087] Where μ is the initial optimization radius, and D is the matrix used to transform the increment;

[0088] (8.5) Calculate ρ, specifically:

[0089]

[0090] (8.5) If Then μ=2μ; if Then μ = 0.5μ;

[0091] (8.6) If ρ is greater than the set threshold v m , it is considered to be approximately feasible and ends;

[0092] (8.7) Let x k+1 =x k +Δx k , k=k+1, return to step (8.4).

[0093] In order to verify the feasibility and effectiveness of the method, the present invention is further described in detail below with reference to examples.

[0094] Table 1 shows the hardware equipment used in the experiment of a multi-UAV cooperative SLAM system based on common view area detection in the present invention.

[0095] Table 1 Hardware configuration of the experiment of the present invention

[0096]

[0097] Figure 2 These are photos collected during the flight of the drone. By calculating the similarity between these images, the results are shown in Table 2. The rows and columns in the table represent different images, and the similarity score of each image with itself is 1. As can be seen from the table, Figure 2 (a) and Figure 2(h) has the highest similarity score, which is about 0.102, while the similarity scores of other images are about 0.025, indicating that the improved common view region detection algorithm of the present invention can successfully detect the common view region.

[0098] Table 2 Similarity scores between images

[0099]

[0100]

[0101] In order to further verify the effect of the improved common view area detection algorithm in the SLAM system, the SLAM system of the present invention is compared with the ORB-SLAM3 system. Table 3 is a comparison of the common view area detection performance of the SLAM system of the present invention and the ORB-SLAM3 system. The table counts the number of successful detections and the average detection time of the SLAM system. It can be seen that there are 20 common areas in the real trajectory, 16 of which are successfully detected by the SLAM system of the present invention, while the ORB-SLAM system only successfully detects 11, which shows that the detection algorithm of the present invention has a higher accuracy. In addition, the detection algorithm of the present invention increases the average detection time by 1.76 milliseconds, which is mainly due to the need to update the word frequency distribution information of the visual words online in real time, and the need to calculate the logarithm when calculating the similarity score, but still meets the efficiency requirements of real-time detection of drones.

[0102] Table 3 Comparison of common view area detection performance between the SLAM system of the present invention and the ORB-SLAM3 system

[0103]

[0104] Figure 3 The figure shows the collaborative flight trajectory of multiple drones. The dotted line in the figure represents the reference trajectory, and the three solid lines represent the flight trajectories of the three drones. As can be seen from the figure, the three drones explored the unknown environment in the same space-time area. The server corrected the posture based on the common view area, and the global map points and the running trajectory were integrated and updated, thereby improving the positioning accuracy of the overall system and each drone.

[0105] Figure 4 The figure is a schematic diagram of the trajectory error of the multi-UAV cooperative SLAM system. The gray dotted line in the figure represents the true trajectory, and the colored solid line represents the trajectory obtained during the operation. It can be seen intuitively that the client map accuracy of the three UAVs is not high, and the global map accuracy of the server is high, indicating the effectiveness of the multi-UAV cooperative SLAM system.

Claims

1. A multi-UAV cooperative SLAM system based on common view area detection, characterized in that: Including the following steps: Step 1: Obtain an image sequence by a vision sensor; Step 2: Use an improved feature extraction algorithm that introduces an adaptive threshold and feature homogenization to extract representative features in the image sequence; Step 3: Input the image sequence obtained in Step 1 into a lightweight YOLO model to predict the target and its category; Step 4: Use a dynamic feature point screening algorithm to eliminate the feature points on dynamic objects in the image data to reduce the interference of dynamic objects; Step 5: According to the gray invariance of adjacent image frames and the neighborhood motion consistency, identify and exclude outliers from the image matching points through an improved RANSAC algorithm; Step 6: Convert the two-dimensional pixels observed by the camera into three-dimensional world coordinates through coordinate transformation to construct a local map of the UAV; Step 7: Take each UAV as a client, and the server receives the local map information of each UAV, and fuses the local maps based on the co-visible area detection algorithm and relative pose calculation of the visual word bag; Step 8: Use an improved L-M solution algorithm for global optimization to improve pose estimation and generate a high-precision global map.

2. A multi-UAV cooperative SLAM system based on common view area detection as described in claim 1, characterized in that: The specific method of using the improved feature extraction algorithm that introduces an adaptive threshold and feature homogenization to extract representative features in the image sequence in Step 2 is as follows: (2.1) In the traditional ORB feature extraction algorithm, an adaptive threshold is used to select FAST as the key points to improve the detection speed of FAST key points; (2.2) Use the BRIEF descriptor composed of binary strings for feature description; (2.3) Use the method of image pyramid to perform multi-scale feature extraction on the feature points, and homogenize the distribution of ORB feature points on the same level of the image hierarchy through a quadtree to improve the problem of uneven distribution of feature points on the same level, ensure that the image has multi-scale rotation invariance, and improve the stability of the SLAM system during complex maneuvers.

3. A multi-UAV cooperative SLAM system based on common view area detection as described in claim 1, characterized in that: The specific method of identifying and excluding outliers from the feature points through an improved RANSAC algorithm in Step 5 is as follows: (3.1) Assume K m is the maximum number of iterations, satisfying: In the formula, η0 is the confidence level, m is the subset size, and ε is the proportion of inliers in the dataset; (3.2) Normalize the coordinates of all matching point pairs and divide the original image frame into small boxes of the same size. Let I be the grayscale value of the image and calculate the grayscale difference ΔI of all matching point pairs in the same area of ​​adjacent frames. j And the average grayscale difference P(I) = ∑ΔI j , set the average grayscale difference P(I) as the threshold; (3.3) In the grayscale difference ΔI j In the box that is less than the set threshold P(I), select the optimal sample points whose Hamming distance and grayscale difference are both less than the threshold P(I) to construct the initial subset; (3.4) According to the gray difference ΔI j Expand the sample set in ascending order, calculate the reprojection error e of the expanded sample point pairs. When e < P(I), it is an inlier; when e > P(I), it is an outlier. (3.5) Count the number of inliers n and compare it with the set inlier number threshold N; if n < N, the iteration count is incremented by 1, and the previous step is repeated; otherwise, update the inlier set and execute the next step; (3.6) Calculate the fundamental matrix F based on all inliers; (3.7) If the number of iterations is greater than the set maximum number of iterations K m , output the interior point set and the basic matrix F, otherwise increase the number of iterations by 1 and return to step (3.4).

4. A multi-UAV cooperative SLAM system based on common view area detection as described in claim 1, characterized in that: The specific method of fusing the local maps based on the co-visible area detection algorithm and relative pose calculation of the visual word bag in Step 7 is as follows: (4.1) Receive the local map information of each UAV; (4.2) The co-visible area detection algorithm based on the visual word bag mainly includes three aspects: establishing a visual dictionary, extracting the word bag feature vector from the image, and similarity calculation. Specifically: (4.2.1) Obtain a tree-structured visual dictionary through a dictionary generation algorithm based on the K-ary tree structure, where each leaf node corresponds to a clustering center, that is, a visual word; When mapping the image features to visual words, search downward level by level from the root node until the most similar clustering center is found; (4.2.2) Each feature point is mapped to a visual word in the dictionary, and then the bag-of-words vector of the image frame is generated; (4.2.3) An improved similarity calculation method is used to improve the calculation accuracy of the similarity between bag-of-words vectors. The improved similarity calculation method is as follows: In the formula, is the bag-of-words vector and The number of words in the union of visual words, coefficient |v a ∪v b |*log(N) is the normalization parameter, word i is the bag-of-words vector and The visual words in the intersection, N is the number of all images in the query dataset, N i Contains the visual word word i The number of visual images; (4.3) The local map information of the UAV is updated and optimized through the common view area detection algorithm, and the relative pose transformation between different local maps is calculated. The local map is transformed through the transformation matrix T A,i Fuse into the global map to generate a global map, specifically: (4.3.1) Assume that UAV A In t i Moment, UAV B In t j Passing the same location M at all times, UAV A and UAVs B The poses in their respective local map coordinate systems are T A，i and T B,j ; at t j Moment, UAV A The pose in its local map is T A,j , then UAV A From i Time to t j The cumulative posture transformation at the moment is: T A，i→j =T A，i+1 T A，i+2 ...T A,j-1 T A，j (3) (4.3.2) Taking the common location M as the reference point, the UAV A In t j The local map information constructed at time t is converted to i In the coordinate system of the moment, the specific transformation matrix T A，i for: T A,i =(T A，i→j ) -1 T A，j (4) (4.3.3) Using the PnP algorithm, the initial camera pose estimate is calculated based on the known map feature points and their corresponding projection points in the camera coordinate system. The optimization algorithm is iteratively executed, and the final camera pose is the optimal pose transformation matrix T * ; (4.3.4) UAV B The local map is used as the reference system, and the UAV is transformed into A Map point p in the local map A Convert to UAV B The corresponding point p′ in the local map reference system A , the conversion formula is: (4.3.5) Convert the map with the common area as the origin in the previous step into the UAV B The map p″ with the initial coordinates as the origin A Point, the conversion formula is: Where, T B,0 , T B,1 …T B，j For UAV B From the initial time to t j The pose transformation matrix at the moment.

5. A multi-UAV cooperative SLAM system based on common view area detection as described in claim 1, characterized in that: The improved LM solution algorithm in step eight is used to perform global optimization to improve the pose estimation, specifically: (5.1) According to the global BA optimization, the reprojection error is minimized to improve the system accuracy. The cost function of BA optimization is: In the formula, e is the error, z is ij is the position ξ i Observation map point p j The data generated, h(ξ i , p j ) is an estimated value; (5.2) Define x = [ξ1, ..., ξ m , p1, ..., p n ] T , given the initial value x0, and the initial optimization radius μ, when an increment Δx is given to the independent variable x, the cost function f grows to: In the formula, F ij represents the partial derivative of the cost function with respect to the camera posture in the current state, Δξ i is the posture ξ i The increment, E ij represents the partial derivative of the function with respect to the landmark position, Δp j For p j The increment of (5.3) In order to improve the solution efficiency, a memory factor is introduced, and the trust region radius λ of the kth iteration is k Set to: In the formula, μ k is an adjustable parameter, F k is the relative decrease of the cost function after each iteration; (5.4) For the kth iteration, solve the following equation: Where μ is the initial optimization radius, and D is the matrix used to transform the increment; (5.5) Calculate ρ, specifically: Here, J(x k ) is the Jacobian matrix; (5.6) If Then μ=2μ; if Then μ = 0.5μ; (5.7) If ρ is greater than the threshold v m , it is considered feasible and ends; (5.8) Let x k+1 =x k +Δx k , k=k+1, return to step (5.4).

Images