Path planning method based on neighborhood node cost estimation

Abstract

The invention discloses a path planning method based on neighborhood node cost estimation, which comprehensively considers smaller positioning precision and shorter path length, takes an inertial sensor as a core sensor, and estimates the current position of a carrier and calculates the current positioning error of the carrier through a factor graph fusion algorithm. Aiming at the problem of navigation positioning precision of a carrier, and a traditional method basically only considers the shortest path in obstacle avoidance, the method comprises the following steps: fusing obstacle (road sign) information with inertial sensor data, estimating the position of the carrier in a planned path, and calculating to obtain a position root-mean-square error of the carrier in the planned path according to the planned path, so that the navigation positioning precision of the carrier is improved. And constructing a position root mean square error cost, and planning the path by taking the position root mean square error cost and the path length as a cost function. The relative information between the obstacle and the carrier and the self-adaptive adjustment capability of the error model are utilized, so that the method is more suitable for practical application scenes, and is suitable for being popularized and used in the fields of unmanned driving, robot navigation, carrier positioning and the like.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation technology, and in particular to a path planning method based on neighborhood node cost estimation. Background Art

[0002] In recent years, path planning is one of the core issues in the field of navigation of robots, driverless cars and other mobile devices. Its goal is to determine an optimal path from the starting position to the target position for the mobile subject, while meeting the path feasibility and task requirements. Traditional path planning schemes basically only consider the shortest path problem in obstacle avoidance, and do not consider the problem of carrier navigation and positioning accuracy. However, in actual environments, positioning errors are inevitable due to sensor noise, environmental interference and uncertainty of multi-source information. These errors may cause the path planning results to lack robustness to environmental changes, thereby reducing the reliability of path planning, and the navigation and positioning accuracy is also related to safety, efficiency, economic benefits and user experience. Summary of the invention

[0003] The technical problem to be solved by the present invention is to provide a path planning method based on neighborhood node cost estimation to address the defects involved in the background technology, which comprehensively considers lower positioning accuracy and shorter path length, so as to achieve higher positioning accuracy while ensuring a shorter path.

[0004] The present invention adopts the following technical solutions to solve the above technical problems:

[0005] A path planning method based on neighborhood node cost estimation includes the following steps:

[0006] Step 1), initializing the starting point, the open list and the closed list, and adding the starting point to the open list, wherein the open list is used to store the nodes to be processed, and the closed list is used to store the processed nodes;

[0007] Step 2), move the starting point from the open list to the closed list, traverse the neighboring nodes of the starting point, add each neighboring node to the open list, set the starting point as its parent node, calculate the comprehensive cost between the neighboring node and the starting point and use the comprehensive cost as the estimated cost of the neighboring node, where the specific steps of calculating the comprehensive cost between node A and its neighboring node n are as follows:

[0008] Step 2.1), calculate the path cost F(n) from node A to node n according to the following formula:

[0009] F(n)=G(n)+H(n)

[0010] Among them, G(n) represents the actual cost function from node A to node n, and H(n) represents the estimated cost from node n to the target node;

[0011] Step 2.2), calculate the root mean square error cost of the carrier position between node A and node n;

[0012] Step 2.2.1), obtain the reference trajectory of the carrier between node A and node n through the RTK system;

[0013] Step 2.2.2), obtain the accelerometer data and gyroscope data of the carrier through the IMU sensor, and build an IMU pre-integration model;

[0014] Step 2.2.2.1), the IMU measurement output is affected by accelerometer bias, gyroscope bias and additive noise, and the acceleration output data is as follows:

[0015]

[0016] in, They are the accelerometer measurement data and gyroscope measurement data output by IMU, respectively. t ,ω t They are the real accelerometer data and gyroscope data, respectively. a 、b ω They are accelerometer bias and gyroscope bias respectively; n a 、n ω are the additive noise measured by the accelerometer and gyroscope,

[0017] Step 2.2.2.2), b a 、b ω Modeled as a random walk whose derivative is Gaussian white noise

[0018]

[0019] Step 3.2.3), define the IMU pre-integration model according to the IMU sensor characteristics:

[0020]

[0021] in, They are t k+1 ,t k The mapping of the IMU position of node n in the navigation coordinate system, They are respectively carrier t k+1 ,t k The speed of the carrier at node n in the navigation coordinate system, is the carrier of node n at t k+1 The posture, They are the quaternion and rotation matrix of the carrier of node n from the navigation coordinate system to the carrier coordinate system, g n is the gravitational acceleration of the carrier of node n in the navigation coordinate system, Δt k Yes k With t k+1 the time interval between They are the pre-integrated terms of the position, velocity, and attitude of the carrier of node n, which are only related to the bias of the IMU;

[0022] Step 2.2.3) Construct the state equation according to the motion state of the carrier Among them, x k Yes k The pose of the carrier to be estimated at time, x k+1 t k+1 The carrier position at the moment; t k Time to t k+1 The IMU pre-integrated data at time; f(·) is the corresponding nonlinear function; w k+1 is the noise added during the motion; l j represents the jth landmark in the environment, is the carrier state set, is the signpost set; t k Status of the moment The vector representing node n is at t k posture;

[0023] Step 2.2.4), define the landmark measurement equation according to the characteristics of the radio sensor and the angle calculation method:

[0024]

[0025] in, is the landmark observation value; r k ,φ k They are the distance and direction between the carrier and the landmark; are the measurement function and measurement noise of the landmark points; theta k is the current carrier heading angle; is the location coordinate of the landmark point; is the current carrier position coordinate;

[0026] Step 2.2.5), construct the IMU pre-integration factor, according to the IMU pre-integration model, consider two consecutive moments t k and t k+1The IMU measurement in the IMU is defined by the residual method, and its expression is:

[0027]

[0028] Then the IMU pre-integration factor is the residual of the IMU pre-integration factor, d(·) represents the cost function corresponding to the factor f(X);

[0029] Step 2.2.6), defining an adaptive landmark factor according to the landmark measurement equation;

[0030] Step 2.2.6.1), with r k,j As a criterion for calculating the weight coefficient, the distance is converted into the initial weight according to the preset distance threshold thes_r

[0031]

[0032] Step 2.2.6.2), normalize the initial weight and use it as the weight of the adaptive landmark factor ρ k :

[0033]

[0034] Among them, NormalFunc(·) is the normalization function, They are the maximum and minimum values ​​of the preset detection range respectively;

[0035] Step 2.2.6.3), measurement noise of adaptive landmark factor for:

[0036]

[0037] Adaptive landmark factor The residuals measured for the landmarks;

[0038] Step 2.2.7), perform multi-sensor factor graph optimization based on the carrier's IMU pre-integration factor and adaptive landmark factor, and calculate the carrier's position root mean square error cost of the neighborhood node;

[0039] Step 2.2.7.1), integrate the IMU pre-integration factor and the adaptive landmark factor to obtain the complete factor model f(X) = f IMU (X)·f LD (X);

[0040] Step 2.2.7.2), assuming that the measured values ​​of each sensor at different times are independent of each other and the noise obeys Gaussian distribution, then each factor is expressed as:

[0041]

[0042] Step 2.2.7.3), by taking the negative logarithm, use the following formula to obtain the maximum a posteriori estimate of the state Thus, the estimated carrier position of the neighborhood node is obtained:

[0043]

[0044] Step 2.2.7.4), calculate the root mean square error cost of the location of the neighborhood nodes according to the following formula:

[0045] E(n)=RMSE(p es (n),p truth (n))

[0046] Where E(n) is the root mean square error cost of the position of the carrier from node A to node n, p es (n) is the carrier position of node n, p truth (n) is the reference trajectory of the carrier of node n, and RMSE(·) is the calculation of p es (n) is the function of the root mean square error,

[0047] Step 2.3), calculate the comprehensive cost I from node A to node n A =αF+(1-α)E, where α is the preset adjustment weight coefficient, F is the path cost of the neighboring node, and E is the root mean square error cost of the carrier position of the neighboring node;

[0048] Step 3), select the node i with the smallest estimated cost from the open list, move the node i from the open list to the closed list, and determine whether the node i is the target node. If the node i is the target node, jump to step 6); otherwise, execute step 4);

[0049] Step 4), traverse the neighboring nodes around node i, for each neighboring node:

[0050] Step 4.1), if the neighboring node is in the closed list, skip the neighboring node;

[0051] Step 4.2), if the neighbor node is not in the open list:

[0052] Step 4.2.1), add the neighboring node to the open list, and record node i as the parent node of the neighboring node;

[0053] Step 4.2.2), calculate the comprehensive cost between the neighboring node and node i, and add the comprehensive cost to the estimated cost of node i as the estimated cost of the neighboring node;

[0054] Step 4.3), if the neighbor node is in the open list:

[0055] Step 4.3.1), calculate the comprehensive cost between the neighboring nodes and node i, and add the comprehensive cost to the estimated cost of node i to obtain the undetermined cost;

[0056] Step 4.3.2), compare the pending cost with the estimated cost of the neighboring node. If the pending cost is less than the estimated cost of the neighboring node, replace the pending cost with the estimated cost of the neighboring node, and record node i as the parent node of the neighboring node;

[0057] Step 5), repeat steps 3) to 4) until the target node is found or the open list is empty;

[0058] Step 6), if the target node is found, the path from the end point to the starting point is recorded by backtracking the parent node and returned as the planned path; if the open list is empty, it means that the planned path is not found.

[0059] As a further optimization scheme of the path planning method based on neighborhood node cost estimation of the present invention, H(n) in step 2.1) is calculated by Euclidean distance, Among them, (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n ,y n ) are the horizontal and vertical coordinates of the current point.

[0060] As a further optimization scheme of the path planning method based on neighborhood node cost estimation of the present invention, H(n) in step 2.1) is calculated by Manhattan distance, H(n)=(|x goal -x n |+|y goal -y n |), where (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n ,y n ) are the horizontal and vertical coordinates of the current point.

[0061] As a further optimization scheme of the path planning method based on neighborhood node cost estimation of the present invention, H(n) in step 2.1) is calculated by Chebyshev distance, H(n)=max(|x goal -x n |,|y goal -y n |), where (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n,y n ) are the horizontal and vertical coordinates of the current point.

[0062] Compared with the prior art, the present invention adopts the above technical solution and has the following technical effects:

[0063] The present invention discloses a planning method based on neighborhood node cost estimation, which introduces a position root mean square error cost into a traditional A* algorithm, and comprehensively considers the shortest path length and a smaller positioning error; the calculation of the position root mean square error cost is implemented using a multi-source fusion algorithm based on a factor graph, and an adaptive landmark factor is used to make the distance measurement value between the carrier and the landmark more reasonable; compared with the traditional A* algorithm, the present invention comprehensively considers the two factors of the shortest path length and the minimum positioning error, and can achieve higher positioning accuracy while ensuring a shorter travel route, and is suitable for practical applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 It is a schematic diagram of the principle flow of the method of the present invention;

[0065] Figure 2 A schematic diagram showing the comparison of path planning results between the present invention and the A* method in a straight line scenario;

[0066] Figure 3 (a) Figure 3 (b) are schematic diagrams of curve comparison of north position error and east position error of the present invention and the A* method during the travel process in a straight line scenario;

[0067] Figure 4 A schematic diagram showing the comparison of the path planning results of the present invention and the A* algorithm in a turning situation;

[0068] Figure 5 (a) Figure 5 (b) are schematic diagrams showing the curve comparison of the north position error and east position error of the present invention and the A* method during the driving process in a turning situation. DETAILED DESCRIPTION

[0069] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings:

[0070] The present invention can be implemented in many different forms and should not be considered to be limited to the embodiments described herein. On the contrary, these embodiments are provided to make this disclosure thorough and complete, and will fully express the scope of the present invention to those skilled in the art. In the accompanying drawings, components are enlarged for clarity.

[0071] like Figure 1 As shown, a planning method based on neighborhood node cost estimation includes the following steps:

[0072] Step 1), initializing the starting point, the open list and the closed list, and adding the starting point to the open list, wherein the open list is used to store the nodes to be processed, and the closed list is used to store the processed nodes;

[0073] Step 2), move the starting point from the open list to the closed list, traverse the neighboring nodes of the starting point, add each neighboring node to the open list, set the starting point as its parent node, calculate the comprehensive cost between the neighboring node and the starting point and use the comprehensive cost as the estimated cost of the neighboring node, where the specific steps of calculating the comprehensive cost between node A and its neighboring node n are as follows:

[0074] Step 2.1), calculate the path cost F(n) from node A to node n according to the following formula:

[0075] F(n)=G(n)+H(n)

[0076] Among them, G(n) represents the actual cost function from node A to node n, and H(n) represents the estimated cost from node n to the target node;

[0077] Step 2.2), calculate the root mean square error cost of the carrier position between node A and node n;

[0078] Step 2.2.1), obtain the reference trajectory of the carrier between node A and node n through the RTK system;

[0079] Step 2.2.2), obtain the accelerometer data and gyroscope data of the carrier through the IMU sensor, and build an IMU pre-integration model;

[0080] Step 2.2.2.1), the IMU measurement output is affected by accelerometer bias, gyroscope bias and additive noise, and the acceleration output data is as follows:

[0081]

[0082] in, They are the accelerometer measurement data and gyroscope measurement data output by IMU, respectively. t ,ω t They are the real accelerometer data and gyroscope data, respectively. a 、b ω They are accelerometer bias and gyroscope bias respectively; n a 、n ω are the additive noise measured by the accelerometer and gyroscope,

[0083] Step 2.2.2.2), b a 、b ωModeled as a random walk whose derivative is Gaussian white noise

[0084]

[0085] Step 3.2.3), define the IMU pre-integration model according to the IMU sensor characteristics:

[0086]

[0087] in, They are t k+1 ,t k The mapping of the IMU position of node n in the navigation coordinate system, They are respectively carrier t k+1 ,t k The speed of the carrier at node n in the navigation coordinate system, is the carrier of node n at t k+1 The posture, They are the quaternion and rotation matrix of the carrier of node n from the navigation coordinate system to the carrier coordinate system, g n is the gravitational acceleration of the carrier of node n in the navigation coordinate system, Δt k Yes k With t k+1 the time interval between They are the pre-integrated terms of the position, velocity, and attitude of the carrier of node n, which are only related to the bias of the IMU;

[0088] Step 2.2.3) Construct the state equation according to the motion state of the carrier Among them, x k Yes k The pose of the carrier to be estimated at time, x k+1 t k+1 The carrier position at the moment; t k Time to t k+1 The IMU pre-integrated data at time; f(·) is the corresponding nonlinear function; w k+1 is the noise added during the motion; l j represents the jth landmark in the environment, is the carrier state set, is the signpost set; t k Status of the moment The vector representing node n is at t k posture;

[0089] Step 2.2.4), define the landmark measurement equation according to the characteristics of the radio sensor and the angle calculation method:

[0090]

[0091] in, is the landmark observation value; r k ,φ k They are the distance and direction between the carrier and the landmark; are the measurement function and measurement noise of the landmark points; theta k is the current carrier heading angle; is the location coordinate of the landmark point; is the current carrier position coordinate;

[0092] Step 2.2.5), construct the IMU pre-integration factor, according to the IMU pre-integration model, consider two consecutive moments t k and t k+1 The IMU measurement in the IMU is defined by the residual method, and its expression is:

[0093]

[0094] Then the IMU pre-integration factor is the residual of the IMU pre-integration factor, d(·) represents the cost function corresponding to the factor f(X);

[0095] Step 2.2.6), defining an adaptive landmark factor according to the landmark measurement equation;

[0096] Step 2.2.6.1), with r k,j As a criterion for calculating the weight coefficient, the distance is converted into the initial weight according to the preset distance threshold thes_r

[0097]

[0098] Step 2.2.6.2), normalize the initial weight and use it as the weight of the adaptive landmark factor ρ k :

[0099]

[0100] Among them, NormalFunc(·) is the normalization function, They are the maximum and minimum values ​​of the preset detection range respectively;

[0101] Step 2.2.6.3), measurement noise of adaptive landmark factor for:

[0102]

[0103] Adaptive landmark factor The residuals measured for the landmarks;

[0104] Step 2.2.7), perform multi-sensor factor graph optimization based on the carrier's IMU pre-integration factor and adaptive landmark factor, and calculate the carrier's position root mean square error cost of the neighborhood node;

[0105] Step 2.2.7.1), integrate the IMU pre-integration factor and the adaptive landmark factor to obtain the complete factor model f(X) = f IMU (X)·f LD (X);

[0106] Step 2.2.7.2), assuming that the measured values ​​of each sensor at different times are independent of each other and the noise obeys Gaussian distribution, then each factor is expressed as:

[0107]

[0108] Step 2.2.7.3), by taking the negative logarithm, use the following formula to obtain the maximum a posteriori estimate of the state Thus, the estimated carrier position of the neighborhood node is obtained:

[0109]

[0110] Step 2.2.7.4), calculate the root mean square error cost of the location of the neighborhood nodes according to the following formula:

[0111] E(n)=RMSE(p es (n),p truth (n))

[0112] Where E(n) is the root mean square error cost of the position of the carrier from node A to node n, p es (n) is the carrier position of node n, p truth (n) is the reference trajectory of the carrier of node n, and RMSE(·) is the calculation of p es (n) is the function of the root mean square error,

[0113]

[0114] Step 2.3), calculate the comprehensive cost I from node A to node n A =αF+(1-α)E, where α is the preset adjustment weight coefficient, F is the path cost of the neighboring node, and E is the root mean square error cost of the carrier position of the neighboring node;

[0115] Step 3), select the node i with the smallest estimated cost from the open list, move the node i from the open list to the closed list, and determine whether the node i is the target node. If the node i is the target node, jump to step 6); otherwise, execute step 4);

[0116] Step 4), traverse the neighboring nodes around node i, for each neighboring node:

[0117] Step 4.1), if the neighboring node is in the closed list, skip the neighboring node;

[0118] Step 4.2), if the neighbor node is not in the open list:

[0119] Step 4.2.1), add the neighboring node to the open list, and record node i as the parent node of the neighboring node;

[0120] Step 4.2.2), calculate the comprehensive cost between the neighboring node and node i, and add the comprehensive cost to the estimated cost of node i as the estimated cost of the neighboring node;

[0121] Step 4.3), if the neighbor node is in the open list:

[0122] Step 4.3.1), calculate the comprehensive cost between the neighboring nodes and node i, and add the comprehensive cost to the estimated cost of node i to obtain the undetermined cost;

[0123] Step 4.3.2), compare the pending cost with the estimated cost of the neighboring node. If the pending cost is less than the estimated cost of the neighboring node, replace the pending cost with the estimated cost of the neighboring node, and record node i as the parent node of the neighboring node;

[0124] Step 5), repeat steps 3) to 4) until the target node is found or the open list is empty;

[0125] Step 6), if the target node is found, the path from the end point to the starting point is recorded by backtracking the parent node and returned as the planned path; if the open list is empty, it means that the planned path is not found.

[0126] As a further optimization scheme of the path planning method based on neighborhood node cost estimation of the present invention, H(n) in step 2.1) is calculated by Euclidean distance, Among them, (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n ,y n ) are the horizontal and vertical coordinates of the current point.

[0127] As a further optimization scheme of the path planning method based on neighborhood node cost estimation of the present invention, H(n) in step 2.1) is calculated by Manhattan distance, H(n)=(|x goal -x n |+|y goal -y n |), where (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n ,y n ) are the horizontal and vertical coordinates of the current point.

[0128] As a further optimization scheme of the path planning method based on neighborhood node cost estimation of the present invention, H(n) in step 2.1) is calculated by Chebyshev distance, H(n)=max(|x goal -x n |,|y goal -y n |), where (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n ,y n ) are the horizontal and vertical coordinates of the current point.

[0129] In order to verify the effectiveness of the planning method based on neighborhood node cost estimation proposed in the present invention, a digital simulation analysis is carried out. Figure 2 The planned path information using the algorithm of the present invention and the A* algorithm in a straight line scenario is given. Figure 3 (a) Figure 3 (b) shows the curve comparison of north position error and east position error of the two algorithms in the straight line scenario. The paths planned by the method of the present invention and the A* algorithm in the turning scenario are shown in Figure 2. Figure 4 shown. Figure 5 (a) Figure 5 (b) Comparison of the north position error and east position error curves of the two algorithms during the driving process in a turning scenario.

[0130] from Figure 3 It can be seen that in the path planning area (25s-45s), the positioning error after using the method of the present invention is significantly reduced. Compared with the A* algorithm, the root mean square error of the north position of the method of the present invention is reduced by 64.81%, and the positioning accuracy of the east position is improved by 14.48%. Figure 4 It can be seen that after considering the positioning error, the path length of the method of the present invention is longer than that of the A* algorithm, but Figure 5It can be seen that the positioning error of the method of the present invention is smaller than that of the A* algorithm. It can be concluded that the method of the present invention using the position root mean square error cost can improve the navigation positioning accuracy compared with the A* algorithm.

[0131] It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as those generally understood by those skilled in the art in the art to which the present invention belongs. It should also be understood that terms such as those defined in common dictionaries should be understood to have meanings consistent with the meanings in the context of the prior art, and will not be interpreted with idealized or overly formal meanings unless defined as herein.

[0132] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A path planning method based on neighborhood node cost estimation, characterized in that: The following steps are involved: Step 1), initializing the starting point, the open list and the closed list, and adding the starting point to the open list, wherein the open list is used to store the nodes to be processed, and the closed list is used to store the processed nodes; Step 2), move the starting point from the open list to the closed list, traverse the neighboring nodes of the starting point, add each neighboring node to the open list, set the starting point as its parent node, calculate the comprehensive cost between the neighboring node and the starting point and use the comprehensive cost as the estimated cost of the neighboring node, where the specific steps of calculating the comprehensive cost between node A and its neighboring node n are as follows: Step 2.1), calculate the path cost F(n) from node A to node n according to the following formula: F(n)=G(n)+H(n) Among them, G(n) represents the actual cost function from node A to node n, and H(n) represents the estimated cost from node n to the target node; Step 2.2), calculate the root mean square error cost of the carrier position between node A and node n; Step 2.2.1), obtain the reference trajectory of the carrier between node A and node n through the RTK system; Step 2.2.2), obtain the accelerometer data and gyroscope data of the carrier through the IMU sensor, and build an IMU pre-integration model; Step 2.2.2.1), the IMU measurement output is affected by accelerometer bias, gyroscope bias and additive noise, and the acceleration output data is as follows: in, They are the accelerometer measurement data and gyroscope measurement data output by IMU, respectively. t ,ω t They are the real accelerometer data, gyroscope data, and b a 、b ω They are accelerometer bias and gyroscope bias respectively; n a 、n ω are the additive noise measured by the accelerometer and gyroscope, Step 2.2.2.2), b a 、b ω Modeled as a random walk whose derivative is Gaussian white noise Step 3.2.3), define the IMU pre-integration model according to the IMU sensor characteristics: in, They are t k+1 ,t k The mapping of the IMU position of node n in the navigation coordinate system, They are respectively carrier t k+1 ,t k The speed of the carrier at node n in the navigation coordinate system, is the carrier of node n at t k+1 The posture, They are the quaternion and rotation matrix of the carrier of node n from the navigation coordinate system to the carrier coordinate system, g n is the gravitational acceleration of the carrier of node n in the navigation coordinate system, Δt k Yes k With t k+1 the time interval between They are the pre-integrated terms of the position, velocity, and attitude of the carrier of node n, which are only related to the bias of the IMU; Step 2.2.3) Construct the state equation according to the motion state of the carrier Among them, x k Yes k The pose of the carrier to be estimated at time, x k+1 t k+1 The carrier position at the moment; t k Time to t k+1 The IMU pre-integrated data at time; f(·) is the corresponding nonlinear function; w k+1 is the noise added during the motion; l j represents the jth landmark in the environment, is the carrier state set, is the signpost set; t k Status of the moment The vector representing node n is at t k posture; Step 2.2.4), define the landmark measurement equation according to the characteristics of the radio sensor and the angle calculation method: in, is the landmark observation value; r k ,φ k They are the distance and direction between the carrier and the landmark; are the measurement function and measurement noise of the landmark points; theta k is the current carrier heading angle; is the location coordinate of the landmark point; is the current carrier position coordinate; Step 2.2.5), construct the IMU pre-integration factor, according to the IMU pre-integration model, consider two consecutive moments t k and t k+1 The IMU measurement in the IMU is defined by the residual method, and its expression is: Then the IMU pre-integration factor is the residual of the IMU pre-integration factor, d(·) represents the cost function corresponding to the factor f(X); Step 2.2.6), defining an adaptive landmark factor according to the landmark measurement equation; Step 2.2.6.1), with r k,j As a criterion for calculating the weight coefficient, the distance is converted into the initial weight according to the preset distance threshold thes_r Step 2.2.6.2), normalize the initial weight and use it as the weight of the adaptive landmark factor ρ k : Among them, NormalFunc(·) is the normalization function, They are the maximum and minimum values ​​of the preset detection range respectively; Step 2.2.6.3), measurement noise of adaptive landmark factor for: Adaptive landmark factor The residuals measured for the landmarks; Step 2.2.7), perform multi-sensor factor graph optimization based on the carrier's IMU pre-integration factor and adaptive landmark factor, and calculate the carrier's position root mean square error cost of the neighborhood node; Step 2.2.7.1), integrate the IMU pre-integration factor and the adaptive landmark factor to obtain the complete factor model f(X) = f IMU (X)·f LD (X); Step 2.2.7.2), assuming that the measured values ​​of each sensor at different times are independent of each other and the noise obeys Gaussian distribution, then each factor is expressed as: Step 2.2.7.3), by taking the negative logarithm, use the following formula to obtain the maximum a posteriori estimate of the state Thus, the estimated carrier position of the neighborhood node is obtained: Step 2.2.7.4), calculate the root mean square error cost of the location of the neighborhood nodes according to the following formula: E(n)=RMSE(p es (n),p truth (n)) Where E(n) is the root mean square error cost of the position of the carrier from node A to node n, p es (n) is the carrier position of node n, p truth (n) is the reference trajectory of the carrier of node n, and RMSE(·) is the calculation of p es (n) is the function of the root mean square error, Step 2.3), calculate the comprehensive cost I from node A to node n A =αF+(1-α)E, where α is the preset adjustment weight coefficient, F is the path cost of the neighboring node, and E is the root mean square error cost of the carrier position of the neighboring node; Step 3), select the node i with the smallest estimated cost from the open list, move the node i from the open list to the closed list, and determine whether the node i is the target node. If the node i is the target node, jump to step 6); otherwise, execute step 4); Step 4), traverse the neighboring nodes around node i, for each neighboring node: Step 4.1), if the neighboring node is in the closed list, skip the neighboring node; Step 4.2), if the neighbor node is not in the open list: Step 4.2.1), add the neighboring node to the open list, and record node i as the parent node of the neighboring node; Step 4.2.2), calculate the comprehensive cost between the neighboring node and node i, and add the comprehensive cost to the estimated cost of node i as the estimated cost of the neighboring node; Step 4.3), if the neighbor node is in the open list: Step 4.3.1), calculate the comprehensive cost between the neighboring nodes and node i, and add the comprehensive cost to the estimated cost of node i to obtain the undetermined cost; Step 4.3.2), compare the pending cost with the estimated cost of the neighboring node. If the pending cost is less than the estimated cost of the neighboring node, replace the pending cost with the estimated cost of the neighboring node, and record node i as the parent node of the neighboring node; Step 5), repeat steps 3) to 4) until the target node is found or the open list is empty; Step 6), if the target node is found, the path from the end point to the starting point is recorded by backtracking the parent node and returned as the planned path; if the open list is empty, it means that the planned path is not found.

2. The path planning method based on neighborhood node cost estimation according to claim 1, characterized in that: H(n) in step 2.1) is calculated by Euclidean distance, Among them, (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n ,y n ) are the horizontal and vertical coordinates of the current point.

3. The path planning method based on neighborhood node cost estimation according to claim 1, characterized in that: The H(n) in step 2.1) is calculated by the Manhattan distance, H(n) = (|x goal -x n |+|y goal -y n |), where (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n ,y n ) are the horizontal and vertical coordinates of the current point.

4. The path planning method based on neighborhood node cost estimation according to claim 1, characterized in that: The H(n) in step 2.1) is calculated by the Chebyshev distance, H(n) = max(|x goal -x n |,|y goal -y n |), where (x goal ,y goal ) is the horizontal and vertical coordinates of the target point, (x n ,y n ) are the horizontal and vertical coordinates of the current point.

Images