A multi-UAV cooperative SLAM system based on common-view region detection

Abstract

This invention discloses a multi-UAV cooperative SLAM system based on common-view region detection. The system first improves the feature extraction algorithm by introducing adaptive thresholding and feature homogenization methods. Second, it reduces the influence of moving outliers by using a feature point selection algorithm that fuses YOLO and geometric constraints. Then, to address the feature mismatch problem in UAV images, an improved feature point selection mechanism is proposed. Next, based on UAV local localization and mapping, a common-view region detection algorithm based on bag-of-visual-words and relative pose calculation are used to fuse the local map. Finally, an improved L-M algorithm is used for global optimization to improve pose estimation and generate a high-precision global map. This invention improves the multi-UAV cooperative SLAM system by reducing interference from moving obstacles in unknown environments, improving the accuracy of mapping and localization, and exhibiting better real-time performance and robustness.

Description

Technical Field

[0001] This invention belongs to the field of flight control, specifically a multi-UAV cooperative SLAM system based on common-view area detection. Technical Background

[0002] Visual SLAM technology for UAVs is an important research area in UAV autonomous navigation. Cooperative SLAM can effectively improve system accuracy, efficiency, and robustness by integrating data from multiple UAVs, enabling information sharing and task allocation. Compared to the limitations of single UAV systems, such as narrow observation range, limited computing power, low execution efficiency, and weak anti-interference capabilities, cooperative SLAM can integrate multi-UAV data and effectively improve system accuracy, efficiency, and robustness through information sharing and task allocation. In recent years, cooperative SLAM systems have been widely used in disaster relief, agriculture, geographic mapping, and virtual reality. For example, in disaster relief, flexible cooperative systems can significantly shorten the time for generating a global map, saving valuable time for search and rescue personnel.

[0003] However, increasingly complex unknown environments and mission requirements place higher demands on the environmental adaptability and autonomous navigation capabilities of UAVs. Cooperative SLAM requires an effective system architecture to ensure data consistency throughout the SLAM system and to be responsible for actual data sharing and effective resource allocation. Schmuck et al. proposed the COVINS cooperative SLAM framework, in which each client retains only basic visual odometry and shares a local map with the server. However, due to the ORB-SLAM3 system used by the clients, computational resources are high, making it difficult to meet real-time requirements. The RAMA-SLAM proposed by Liu et al. considers resource contention and effectively reduces the computational load and complexity of cooperative localization algorithms by decoupling the server-side loop closure detection and the client-side VIO module. However, the above cooperative SLAM works often ignore dynamic and complex real-world scenes. In real-world scenes, there are often many moving objects. Traditional visual SLAM systems usually extract invariant features in the scene, such as corners and edges, and use these to infer the camera's own pose changes. When there are dynamic objects in the scene, feature points are likely to be distributed on the dynamic objects and change with the movement of the objects. In this case, it is difficult for the camera to determine whether the change is due to scene changes or its own pose changes. Bescos et al.'s DynaSLAM uses a Mask R-CNN-based detection method to improve system accuracy, but its pixel-segmentation-based model leads to inefficiency. Wu et al. used the more efficient YOLO object detection model, which to some extent compensated for the inefficiency, but it cannot handle potentially moving objects. Furthermore, cooperative SLAM must consider practical issues such as bandwidth, communication latency, and packet loss in real-world environments. Summary of the Invention

[0004] The purpose of this 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 dynamic environments, reduce the consumption of client computing resources, and improve real-time performance.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A multi-UAV cooperative SLAM system based on shared-view region detection includes:

[0007] Step 1: Obtain image sequences using a visual sensor;

[0008] Step 2: An improved feature extraction algorithm incorporating adaptive thresholding and feature homogenization is used to extract representative features from the image sequence.

[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 four: Use a dynamic feature point filtering algorithm to remove feature points on dynamic objects in the image data to reduce interference from dynamic objects;

[0011] Step 5: Based on the grayscale invariance and neighborhood motion consistency of adjacent image frames, the improved RANSAC algorithm is used to identify matching points in the image and exclude outliers.

[0012] Step 6: The two-dimensional pixels observed by the camera are transformed into three-dimensional world coordinates through coordinate transformation to construct a local map of the UAV.

[0013] Step 7: Treat each drone as a client, and have the server receive local map information from each drone. The local map is then fused based on the common-view region detection algorithm using the bag-of-words visual algorithm and relative pose calculation.

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

[0015] The present invention has the following advantages:

[0016] 1. This invention extracts semantic information from UAV images by constructing a lightweight target detection network, filters dynamic objects by combining target detection and geometric constraints, and further reduces the feature mismatch rate by adopting a feature filtering mechanism. This reduces the computational burden and improves the accuracy of the system, making it more suitable for performing localization and mapping tasks in dynamic environments.

[0017] 2. This invention enhances the data association between UAVs by using a common-view region detection algorithm, and improves the accuracy of global map and pose estimation by calculating the relative pose fusion local map between UAVs and using an improved LM solution algorithm for global optimization, thereby improving the real-time performance and robustness. Attached Figure Description

[0018] Figure 1 This is a flowchart of the method of the present invention.

[0019] Figure 2 These are photos taken during the flight of a drone.

[0020] Figure 3 For the collaborative flight path of multiple drones.

[0021] Figure 4 A schematic diagram of trajectory error in a multi-UAV collaborative SLAM system. Detailed Implementation

[0022] The technical solution of the present invention will be described in detail with reference to the accompanying drawings and tables.

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

[0024] Step 1: Obtain a raw image sequence of size 640×640×3 using a visual sensor;

[0025] Step 2 employs an improved feature extraction algorithm that incorporates adaptive thresholding and feature homogenization to extract representative features from the image sequence. Specifically:

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

[0027] (2.2) The features are described using the BRIEF descriptor, which consists of binary strings;

[0028] (2.3) The image pyramid method is used to extract features at multiple scales. The distribution of ORB feature points at the same level is homogenized by quadtree, which improves the problem of uneven distribution of feature points at the same level, ensures that the image is non-deformable during multi-scale rotation, and improves the stability of the SLAM system in complex maneuvers.

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

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

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

[0032] (3.3) The Neck part is based on the idea of ​​feature pyramid network. It integrates feature maps through top-down paths and lateral connections to capture three-scale target information.

[0033] (3.4) In the Prediction part, a multi-scale detection strategy is adopted to generate feature maps of three scales: 13×13, 26×26, and 52×52. By dividing the image into grids corresponding to its size, the prediction training results are output, namely the position and category information of the target in the image.

[0034] Step 4 employs a dynamic feature point filtering algorithm to remove feature points on dynamic objects in the image, reducing interference from dynamic objects for the drone. Specifically:

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

[0036] (4.2) Let the feature point on the previous frame be P1 and the feature point on the current frame be P2. Determine the epipolar line, that is, the projection line of the ray determined by the two points in space onto the image plane where the two feature points are located.

[0037] (4.3) Calculate the distance from the feature point to the epipolar line, and let the distances from the two feature points to the epipolar line be d and d′, respectively;

[0038] (4.4) If d+d′>τ, then p1 and p2 are considered to be dynamic feature points in the image data and are removed;

[0039] (4.5) If d+d′<τ, then P1 and P2 are considered to be static feature points in the image data and are retained;

[0040] (4.6) Update the set of interior points to obtain the final correct static feature point pair P′1 and P′2.

[0041] Step 5, based on the grayscale invariance of adjacent image frames and neighborhood motion consistency, identifies matching points in the image and excludes outliers using an improved RANSAC algorithm. Specifically:

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

[0043]

[0044] In the formula, η0 is the confidence level, m is the subset size, and ε is the proportion of inliers in the dataset;

[0045] (5.2) Normalize the coordinates of all matching point pairs, and divide the original image frame into small boxes of the same size. Calculate the grayscale difference ΔI between all matching point pairs within the same region of adjacent frames. j The average gray level difference P(I) = ∑ΔI j Set the average grayscale difference as a threshold;

[0046] (5.3) In the grayscale difference ΔI j Within the bounding box where both the Hamming distance and gray-level difference are less than the set threshold P(I), the optimal sample points are selected as the initial sample set, and the fundamental matrix F is calculated, specifically satisfying:

[0047]

[0048] In the formula, A1 and A2 refer to the inherent parameter matrices of the two cameras, one before and one after.

[0049] (5.4) Based on the grayscale difference ΔI j The sample set is expanded in ascending order, the reprojection error of the expanded sample point pairs is calculated, and the inlier set is filtered according to the set threshold P(I).

[0050] (5.5) Update the inlier set. If the current inlier size is greater than the minimum number of inliers N required by the improved RANSAC algorithm, output the inlier set and the fundamental matrix F. Otherwise, repeat the previous step.

[0051] Step 6 transforms the 2D pixels observed by the camera into 3D world coordinates to construct a local map of the UAV. The coordinate transformation formula is as follows:

[0052]

[0053] In the formula, Z c Here, (u, v) represents the camera coordinates, (u, v) represents the pixel coordinates, u0 and v0 represent the coordinates of O2 in the pixel coordinate system, dU and dV are both in meters per pixel, f is the camera's focal length, R is the rotation matrix, t is the translation vector, (x, y, z) represents the camera coordinates, and (X, Y, Z) represents the world coordinates. x =f / dU,f y =f / dV,c x =u0,c y=v0.

[0054] Step 7 treats each drone as a client, with the server receiving local map information from each drone. The local map is then fused using a shared-view region detection algorithm based on bag-of-words visual terminology and relative pose calculation. Specifically:

[0055] (7.1) Receive local map information for each UAV;

[0056] (7.2) Improve the co-visual region detection algorithm based on visual bag-of-words, mainly including three aspects: establishing a visual dictionary, extracting bag-of-words feature vectors from images, and similarity calculation, specifically:

[0057] (7.2.1) The dictionary generation algorithm based on the K-ary tree structure produces a tree-structured dictionary, where each leaf node corresponds to a cluster center, i.e., a visual word. When it is necessary to map image features to visual words, the algorithm searches downwards from the root node until the most similar cluster center is found;

[0058] (7.2.2) Each feature point corresponds to a visual word in the dictionary, thereby generating the bag-of-words vector of the image frame;

[0059] (7.2.3) An improved similarity calculation method is adopted to enhance the accuracy of similarity calculation between bag-of-words vectors. The improved similarity calculation formula is as follows:

[0060]

[0061] In the formula, Bag-of-words vectors and The number of words in the visual word union, and the coefficient |v a ∪v b |*log(N) is the normalization parameter, word i Bag-of-words vectors and The visual words in the intersection, where N is the number of images in the query dataset. i For words containing visual words i The number of visual images.

[0062] (7.3) Update and optimize the local map information of the UAV through the common-view region detection algorithm, calculate the relative pose transformation between different local maps, and transform the local map through the transformation matrix T. A，i Integrating into the global map, specifically:

[0063] (7.3.1) Unmanned Aerial Vehicles (UAVs) A In t i At that moment, UAV B In tj UAV passes through the same location M at all times A and UAV B Their poses in their respective local map coordinate systems are T. A，i and T B,j In t j At that moment, UAV A Its pose in the local map is T A,j So, UAV A From t i Time to t j The cumulative pose transformation at each time step is:

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

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

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

[0067] (7.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 final optimized camera pose is the optimal pose transformation matrix T. * ;

[0068] (7.3.4) Using UAV B Using the local map as a reference frame, the UAV is transformed using the optimal pose transformation matrix. A Map point p in a local map A Convert to UAV B The corresponding point p′ in the local map reference frame A The conversion formula is:

[0069]

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

[0071]

[0072] In the formula, T B,0 T B,1 ...T B，j For UAV B From the initial time to t j The pose transformation matrix at time t.

[0073] (7.3.6) By calculating the relative pose and performing map fusion, a unified global map and accurate pose estimation were obtained, and the localization and mapping were initially completed.

[0074] Step 8: An improved LM (Low-Range Metal) algorithm is used for global optimization to enhance pose estimation and generate a high-precision global map. Specifically:

[0075] (8.1) Based on global BA optimization, minimize the reprojection error to improve system accuracy. The specific cost function of BA optimization is as follows:

[0076]

[0077] In the formula, z ij For the in-position ξ i Observe map point p j The generated data;

[0078] (8.2) Define the cost function with independent variable x = [ξ1, ..., ξ2]. m p1, ..., p n ] T Given an initial value x0 and an initial optimization radius μ, when the independent variable x is given an increment Δx, the function grows as follows:

[0079]

[0080] In the formula, F ij E represents the partial derivative of the cost function with respect to the camera pose in the current state. ij This represents the partial derivative of the function with respect to the location of the landmark point;

[0081] (8.3) To improve the solution efficiency, a memory factor is introduced, which is used to determine the radius of the trust region λ in the k-th iteration. k Set to:

[0082]

[0083] In the formula, μ k F is an adjustable parameter that changes according to the relative decrease in the objective function after each iteration. k This represents the relative decrease in the cost function after each iteration;

[0084] (8.4) For the k-th iteration, solve the following equation:

[0085]

[0086] In the formula, μ is the initial optimization radius, and D is the matrix used to transform the increment;

[0087] (8.5) Calculate ρ, specifically:

[0088]

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

[0090] (8.6) If ρ is greater than the set threshold v m Considering it approximately feasible, the process ends.

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

[0092] To verify the feasibility and effectiveness of the method, the present invention will be described in further detail below with reference to examples.

[0093] Table 1 shows the experimental hardware equipment in a multi-UAV cooperative SLAM system based on common-view area detection according to the present invention.

[0094] Table 1 Hardware configuration for the experiment of this invention

[0095]

[0096] Figure 2 These are photos taken during the drone's flight. The similarity between these images was calculated, and the results are shown in Table 2. The rows and columns in the table represent different images, and each image has a similarity score of 1 to itself. As can be seen from the table, Figure 2 (a) and Figure 2 The similarity score of (h) is the highest, at approximately 0.102, while the similarity scores of other images are around 0.025, indicating that the improved co-viewing region detection algorithm of the present invention can successfully detect co-viewing regions.

[0097] Table 2. Similarity scores between images

[0098]

[0099]

[0100] To further verify the effectiveness of the improved common-view region detection algorithm in the SLAM system, a comparative experiment was conducted between the SLAM system of this invention and the ORB-SLAM3 system. Table 3 shows the performance comparison of common-view region detection between the SLAM system of this invention and the ORB-SLAM3 system. The table summarizes the number of successful detections and the average detection time of the SLAM systems. It can be seen that there are 20 common regions in the real trajectory. The SLAM system of this invention successfully detected 16, while the ORB-SLAM system only successfully detected 11. This indicates that the detection algorithm of this invention has a higher accuracy. In addition, the detection algorithm of this invention increases the average detection time by 1.76 milliseconds. This is mainly due to the need to update the word frequency distribution information of visual words online in real time, and the need to calculate the logarithm when calculating the similarity score, but it still meets the efficiency requirements of real-time detection by UAVs.

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

[0102]

[0103] Figure 3 This diagram shows the collaborative flight paths of multiple drones. The dashed line represents the reference trajectory, and the three solid lines represent the flight paths of the three drones. As can be seen from the diagram, the three drones explored the unknown environment within the same spatiotemporal area. The server corrected the pose based on the shared view area, and the global map points and flight trajectories were merged and updated, thereby improving the overall positioning accuracy of the system and the positioning accuracy of each drone.

[0104] Figure 4 This diagram illustrates the trajectory error of a multi-UAV collaborative SLAM system. The gray dashed lines represent the actual trajectory, while the colored solid lines represent the trajectory obtained during operation. It is clearly visible that the client-side map accuracy for the three UAVs is low, while the global map accuracy on the server side is high, demonstrating the effectiveness of the multi-UAV collaborative SLAM system.

Claims

1. A multi-UAV cooperative SLAM system based on common-view region detection, characterized in that, The method comprises the following steps: Step one, obtaining an image sequence by a visual sensor; Step two, extracting features of representative features in the image sequence by using an improved feature extraction algorithm introducing adaptive threshold and feature uniformization; Step three, inputting the image sequence obtained in step one into a light YOLO model to predict the target and its category; Step four, removing feature points on dynamic objects in the image data by using a dynamic feature point screening algorithm to reduce the interference of dynamic objects; Step five, identifying and excluding outliers of the image matching points by using an improved RANSAC algorithm according to the gray invariance of adjacent image frames and the neighborhood motion consistency; Step six, converting two-dimensional pixels observed by a camera into three-dimensional world coordinates through coordinate transformation to construct a local map of the unmanned aerial vehicle; Step seven, taking each unmanned aerial vehicle as a client, receiving local map information of each unmanned aerial vehicle by a server, and fusing the local maps based on a visual bag-of-words co-view area detection algorithm and relative pose calculation; Step eight, performing global optimization by using an improved L-M solving algorithm to improve pose estimation and generate a high-precision global map.

2. The multi-UAV cooperative SLAM system based on common-view region detection of claim 1, wherein, In the step two, the improved feature extraction algorithm introducing adaptive threshold and feature uniformization is used to extract features of representative features in the image sequence, and the specific method is as follows: (2.1) In the traditional ORB feature extraction algorithm, adaptive threshold is used to select FAST as a key point to improve the detection speed of the FAST key point; (2.2) A BRIEF descriptor composed of binary strings is used for feature description; (2.3) The method of image pyramid is used for multi-scale feature extraction of the feature points, the ORB feature points on the same level are uniformly distributed on the image level by using a quadtree, the problem of uneven distribution of feature points on the same level is improved, the image is ensured to have multi-scale rotation invariance, and the stability of the SLAM system in the complex maneuvering process is improved.

3. The multi-UAV cooperative SLAM system based on common view area detection of claim 1, wherein, In the step five, the feature points are identified and outliers are excluded by using the improved RANSAC algorithm, and the specific method is as follows: (3.1) Assume K m is the maximum number of iterations, satisfying: In the formula, η0 is the confidence, m is the subset size, and ε is the proportion of inliers in the data set; (3.2) Normalizing all the matching point pair coordinates, and dividing the original image frame into small boxes of the same size, setting I as the gray value of the image, calculating the gray difference ΔI of all the matching point pairs in the same region of the adjacent frames j and the average gray difference P(I) =∑ΔI j Setting the average gray difference P(I) as the threshold value; (3.3) In the gray scale difference ΔI j Within the box of less than the set threshold P(I), the optimal sample points with both Hamming distance and gray scale difference less than the threshold P(I) are selected to construct the initial subset; (3.4) According to the gray scale difference ΔI j Incrementally expanding the sample set, calculating the re-projection error e of the expanded sample point pair, when e < P(I), it is an inner point, when e > P(I), it is an outer point; (3.5) The number of inliers n is counted and compared with the set threshold N of inliers; if n < N, the iteration number is increased by 1, and the above step is repeated; otherwise, the inlier set is updated, and the next step is performed; (3.6) The basis matrix F is calculated according to all inliers; (3.7) If the iteration number is greater than the set maximum iteration number K m , output the inlier set and the fundamental matrix F, otherwise, increase the iteration number by 1 and return to step (3.4).

4. The multi-UAV cooperative SLAM system based on common view area detection of claim 1, wherein, In the step seven, the local maps are fused based on the visual bag-of-words co-view area detection algorithm and the relative pose calculation, and the specific method is as follows: (4.1) Receiving local map information of each unmanned aerial vehicle; (4.2) The visual bag-of-words co-view area detection algorithm mainly includes three aspects of establishing a visual dictionary, extracting a bag-of-words feature vector from an image, and similarity calculation, and the specific method is as follows: (4.2.1) A tree-structured visual dictionary is obtained based on a K-ary tree structure dictionary generation algorithm, wherein each leaf node corresponds to a cluster center, i.e., a visual word; When the image features need to be mapped to visual words, search from the root node to the lower level until the most similar cluster center is found; (4.2.2) Each feature point is corresponded to a visual word in the dictionary, and then a 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 the bag-of-words vectors, and the improved similarity calculation method is specifically: where, 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 a normalization parameter, word i is the bag-of-words vector and the number of words in the intersection of visual words, N is the number of images in the query dataset, N i is the number of visual images containing visual word word i ; (4.3) Update the local map information of the optimization UAV through the co-sight area detection algorithm, calculate the relative pose transformation between different local maps, and transform the local map through the transformation matrix T A,i Fusion into the global map, generate a global map, specifically: (4.3.1) Assuming a UAV, UAV A At time t i , UAV B passes the same location M j , the poses of UAV A and UAV B in their respective local map coordinate systems are T A，i and T B,j , respectively; at time t j , the pose of UAV A in its local map is T A,j , then the cumulative pose transformation of UAV A from time t i to time t j is: T A，i→j = T A，i+1 T A，i+2 ... T A,j-1 T A，j (3) (4.3.2) With the common point M as the reference point, the UAV A at time t j is converted to the coordinate system at time t i , and the specific transformation matrix T A，i is: T A,i = (T A，i→j ) -1 T A，j (4) (4.3.3) Using PnP algorithm, according to the known map feature points and their corresponding projection points in the camera coordinate system, calculate the initial camera pose estimation, iteratively execute the optimization algorithm, and the final camera pose is the optimal pose transformation matrix T * ; (4.3.4) taking the local map of the UAV B as the reference frame, converting the map point p A in the local map of the UAV A to the corresponding point p′ B in the local map reference frame of the UAV A through the optimal pose transformation matrix, the conversion formula is: (4.3.5) Transforming the map in the previous step, with the common area as the origin, into a UAV B map p" with the initial coordinates as the origin A point, the conversion formula is: In the formula, T B,0 , T B,1 …T B，j is the pose transformation matrix of the UAV B from the initial moment to the moment t j .

5. The multi-UAV cooperative SLAM system based on common view area detection of claim 1, wherein, In the step eight, the improved L-M solving algorithm is used for global optimization to improve the pose estimation, and the specific process is: (5.1) According to the global BA optimization, the re-projection error is minimized to improve the system accuracy, and the cost function of the BA optimization is: where e is the error, z ij is the pose of the robot at time t i is the map point p j generated data, h(ξ i , p j ) is the predicted value; (5.2) Define x = [ξ1,..., ξ m , p1,..., p n ] T , given an initial value x0, and an initial optimization radius μ, when an increment Δx is given to the argument x, the cost function f grows as: where F ij represents the partial derivative of the cost function with respect to the camera pose at the current state, Δξ i is the increment of the pose ξ i , E ij represents the partial derivative of the function with respect to the position of the landmark point, Δp j is the increment of p j ; (5.3) To improve the efficiency of solving, the memory factor is introduced, and the radius of the trust region λ k is set to: where μ k is an adjustable parameter, F k is the relative drop in the cost function after each iteration; (5.4) For the kth iteration, the following formula is solved: In the formula, μ is an initial optimization radius, and D is a matrix used to convert the increment; (5.5) Calculate ρ, and the specific process is: Here, J(x k ) is the Jacobian matrix; (5.6) if then μ = 2μ; if then μ = 0.5μ; (5.7) If p is greater than a threshold value v m then it is considered feasible, and the process ends. (5.8) Let x k+1 = x k + Δx k , k = k + 1, go to (5.4).

Images