Unmanned vehicle positioning estimation method and system fused with passive observation information
By calculating the degree of collaboration in unmanned vehicles and integrating external observation information, the problems of positioning accuracy and system stability of unmanned vehicles in complex environments are solved, and more efficient and reliable positioning estimation is achieved.
Patent Information
- Application Number
- CN202411724869.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-11-28
- Publication Date
- 2025-04-22
AI Technical Summary
How to effectively integrate unmanned vehicles with their own sensor information and external passive observation information in complex environments, improve positioning accuracy and system stability, and avoid the divergence of nonlinear optimization solvers.
By obtaining the positioning state parameters of the unmanned vehicle itself, the observation measurement and uncertainty matrix of external observation points, the degree of cooperation is calculated, and the observation measurements with the degree of cooperation are included in the nonlinear optimization equation to perform cooperative positioning to improve positioning accuracy.
The positioning accuracy and system stability of unmanned vehicles are significantly improved, and the calculation overhead caused by inaccuracy of external observation information and divergence problems of nonlinear optimization solvers are avoided.
Smart Images
Figure CN119860785A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of information processing, and in particular relates to a method and system for estimating the positioning of an unmanned vehicle by integrating passive observation information. Background Art
[0002] The system architecture of driverless vehicles can be divided into three main modules: perception, decision-making, and control. Among them, the perception module is located at the top of the entire system architecture and undertakes a crucial task. Its main function is to accurately perceive and locate the environment around the vehicle through various sensors carried by the vehicle itself, such as lidar, cameras, etc., as well as information obtained from the external environment. After the perception module processes this information, it passes the results to the downstream decision-making and control modules for further analysis and execution.
[0003] In the perception module of an unmanned vehicle, the vehicle uses data collected by various sensors combined with a nonlinear optimization solver to iteratively solve the vehicle's position and posture. This process enables the unmanned vehicle to obtain its own position and posture information in the environment in real time. However, the positioning accuracy of the unmanned vehicle has a decisive influence on its performance in executing tasks. The quality of positioning is directly related to whether the unmanned vehicle can complete its tasks safely and efficiently in a complex and changing environment.
[0004] In addition to the information provided by the vehicle's own sensors, unmanned vehicles can also use other observation sources to improve the accuracy of positioning. These observation sources may include other vehicles, robots, drones, etc., which can provide additional observation information and help improve the positioning accuracy of unmanned vehicles. However, in practical applications, the information provided by these external observation sources is not always completely accurate. Sometimes, this information conflicts greatly with the position and posture solved by the unmanned vehicle itself through sensors. In extreme cases, this conflict may even cause the nonlinear optimization solver of the unmanned vehicle to diverge, thereby affecting the normal operation of the vehicle.
[0005] Therefore, in the system design of unmanned vehicles, how to judge and choose whether to incorporate the observations provided by other information sources into the nonlinear optimization solver of unmanned vehicles becomes a crucial issue. This requires the system to have a certain intelligent judgment ability to evaluate the reliability and accuracy of external observation information and decide whether to include it in the positioning calculation process. In this way, unmanned vehicles can maximize the use of various available resources while ensuring positioning accuracy, thereby improving the stability and reliability of the overall system. Summary of the invention
[0006] Technical solution, in order to solve the technical problems in the above-mentioned background technology, a method and system for unmanned vehicle positioning estimation that integrates passive observation information is proposed, which is used to solve the problem of whether the unmanned vehicle cooperates with the observation source for positioning, and improves the positioning accuracy of both the unmanned vehicle and the observation source.
[0007] The first object of the present invention is to provide a method for estimating the positioning of an unmanned vehicle by integrating passive observation information, comprising:
[0008] Obtain the positioning state parameters of the unmanned vehicle itself; the positioning state parameters include: the current position of the unmanned vehicle itself P1 (x1, y1, α1), and the uncertainty matrix S1 describing the positioning of the unmanned vehicle itself at the current moment, wherein x1 and y1 are the horizontal and vertical coordinates in the positioning scene, and α1 is the heading angle;
[0009] Obtain the observation values of the unmanned vehicle from the relevant observation points and the uncertainty matrix of the positioning of the unmanned vehicle from the n observation points (S2···S n+1 ), where: n is the number of relevant observation points, and n is a natural number greater than 0;
[0010] Based on S1 and (S2...S n+1 ) Calculate the degree of collaboration (C2···C n+1 );
[0011] The degree of cooperation is greater than the cooperation threshold C min The observations are substituted into the nonlinear optimization equation of the unmanned vehicle to obtain the final positioning result.
[0012] Preferably, the acquisition of the observation amount of the unmanned vehicle by the relevant observation points and the uncertainty matrix (S2···S n+1 )include:
[0013] Definition of related observation points The positioning method of the unmanned vehicle is the Euclidean distance d between the two and the relative angle β of the observation. The coordinates of the unmanned vehicle are expressed by the following formula:
[0014]
[0015] Where: i is a natural number between 1 and n, is the value of the x-coordinate inferred by the i-th observation point based on its own positioning point and observation value; is the y coordinate value inferred by the i-th observation point based on its own positioning point and observation value; x i ,y i , α iis the coordinate system positioning information of the i-th observation point itself; d (i1) is the Euclidean distance between the Nth observation point and the unmanned vehicle; β (i1) is the observation angle of the i-th observation point to the unmanned vehicle;
[0016] To find the Jacobian matrix for the above formula, we can get:
[0017]
[0018] Where: γ=α i +β (i1) ; γ represents the sum of the observation angle of the ith observation point relative to the unmanned vehicle and the self-positioning angle of the unmanned vehicle;
[0019] According to the transitivity of the uncertainty matrix, the uncertainty matrix S of the observation quantity of the unmanned vehicle at the i-th related observation point is i+1 It can be expressed by the following formula:
[0020]
[0021] Preferably, the said based on said S1 and (S2...S n+1 ) Calculate the degree of collaboration (C2···C n+1 ) includes: S1 and (S2···S n+1 ), the higher the degree of cooperation between the two parties, the higher the similarity of the positioning posture of the unmanned vehicle. min Set as the minimum collaborative threshold for cooperative positioning; the S1 matrix of the positioning uncertainty of the unmanned vehicle itself is represented by its position uncertainty ellipse, M1 is the area of the position uncertainty ellipse of the unmanned vehicle itself, and M i is the uncertainty ellipse of the pose estimation of the unmanned vehicle for the i-th related measurement point, M 1i is the intersection area of the two, and defines the positioning collaboration C between the two i+1 , C i+1 It is expressed as:
[0022]
[0023] Preferably, the degree of cooperation is greater than the cooperation threshold C min Substitute the observations into the nonlinear optimization equation of the unmanned vehicle, and the final positioning results include: Assuming that the degree of cooperation between the i-th related observation point and the unmanned vehicle is greater than the set threshold C min , the two cooperate to locate, with relative observation as the observation value of the nonlinear optimization solver, and the pose calculated by their own positioning algorithm as the predicted value. The predicted pose is:
[0024]
[0025] In the formula, P1, P i+1 is the predicted position of the unmanned vehicle and the i-th related observation point, is the matrix of the two;
[0026] Uncertainty matrix of predicted pose:
[0027]
[0028] At this time, the observation equation of the two is based on relative observation values:
[0029]
[0030] Right now:
[0031]
[0032] Where: h() is the observation equation of the two, t is the relative observation noise, R 1i is the uncertainty matrix of noise t;
[0033] The Jacobian matrix of the observation equation is obtained:
[0034]
[0035] Where: dx 1i+1 =x i+1 -x1;dy 1i+1 =y i+1 -y1;
[0036] The gain of the nonlinear solver is found to be:
[0037]
[0038] The pose under cooperative positioning is:
[0039]
[0040] Where:
[0041]
[0042] The uncertainty matrix of the pose is updated as:
[0043]
[0044] Finally, P1 and P can be obtained by calculation i+1 .
[0045] The second invention purpose of this patent is to provide an unmanned vehicle positioning estimation system that integrates passive observation information, including:
[0046] The first parameter acquisition module acquires the positioning state parameters of the unmanned vehicle itself; the positioning state parameters include: the current position of the unmanned vehicle itself P1 (x1, y1, α1), and the uncertainty matrix S1 describing the positioning of the unmanned vehicle itself at the current moment, wherein x1 and y1 are the horizontal coordinate and the vertical coordinate in the positioning scene, and α1 is the heading angle;
[0047] The second parameter acquisition module obtains the observation amount of the unmanned vehicle from the relevant observation points and the uncertainty matrix of the positioning of the unmanned vehicle from the n observation points (S2···S n+1 ), where: n is the number of relevant observation points, and n is a natural number greater than 0;
[0048] The calculation module is based on S1 and (S2···S n+1 ) Calculate the degree of collaboration (C2···C n+1 );
[0049] The estimation module calculates the degree of cooperation to be greater than the cooperation threshold C min The observations are substituted into the nonlinear optimization equation of the unmanned vehicle to obtain the final positioning result.
[0050] Preferably, the second parameter acquisition module includes:
[0051] Definition of related observation points The positioning method of the unmanned vehicle is the Euclidean distance d between the two and the relative angle β of the observation. The coordinates of the unmanned vehicle are expressed by the following formula:
[0052]
[0053] Where: i is a natural number between 1 and n, is the value of the x-coordinate inferred by the i-th observation point based on its own positioning point and observation value; is the y coordinate value inferred by the i-th observation point based on its own positioning point and observation value; x i ,y i , α i is the coordinate system positioning information of the i-th observation point itself; d (i1) is the Euclidean distance between the i-th observation point and the unmanned vehicle; β (i1) is the observation angle of the i-th observation point to the unmanned vehicle;
[0054] To find the Jacobian matrix for the above formula, we can get:
[0055]
[0056] Where: γ=α i +β (i1) ; γ represents the sum of the observation angle of the ith observation point relative to the unmanned vehicle and the self-positioning angle of the unmanned vehicle;
[0057] According to the transitivity of the uncertainty matrix, the uncertainty matrix Si+1 of the observation quantity of the unmanned vehicle at the i-th related observation point can be expressed as follows:
[0058]
[0059] Preferably, the calculation module includes: S1 and (S2...S n+1 ), the higher the degree of cooperation between the two parties, the higher the similarity of the positioning posture of the unmanned vehicle. min Set as the minimum collaborative threshold for cooperative positioning; the S1 matrix of the positioning uncertainty of the unmanned vehicle itself is represented by its position uncertainty ellipse, M1 is the area of the position uncertainty ellipse of the unmanned vehicle itself, and M i+1 is the uncertainty ellipse of the pose estimation of the unmanned vehicle for the i-th related measurement point, M 1i is the intersection area of the two, and defines the positioning collaboration C between the two i+1 , C i+1 It is expressed as:
[0060]
[0061] Preferably, the estimation module comprises: assuming that the degree of cooperation between the nth related observation point and the unmanned vehicle is greater than a set threshold C min , the two cooperate to locate, with relative observation as the observation value of the nonlinear optimization solver, and the pose calculated by their own positioning algorithm as the predicted value. The predicted pose is:
[0062]
[0063] In the formula, P1, P i+1 is the predicted position of the unmanned vehicle and the i-th related observation point, is the matrix of the two;
[0064] Uncertainty matrix of predicted pose:
[0065]
[0066] At this time, the observation equation of the two is based on relative observation values:
[0067]
[0068] Right now:
[0069]
[0070] Where: h() is the observation equation of the two, t is the relative observation noise, R 1i is the uncertainty matrix of noise t;
[0071] The Jacobian matrix of the observation equation is obtained:
[0072]
[0073] Where: dx 1i+1 =x i+1 -x1;dy 1i+1 =y i+1 -y1;
[0074] The gain of the nonlinear solver is found to be:
[0075]
[0076] The pose under cooperative positioning is:
[0077]
[0078] Where:
[0079]
[0080] The uncertainty matrix of the pose is updated as:
[0081]
[0082] Finally, P1 and P can be obtained by calculation i+1 .
[0083] The third invention objective of this patent is to provide a computer program product, including a computer program, which, when executed by a processor, implements the above-mentioned positioning estimation method that integrates passive observation information.
[0084] The fourth invention objective of this patent is to provide an information data processing terminal for an unmanned vehicle positioning estimation method that integrates passive observation information.
[0085] The fifth invention objective of this patent is to provide a computer-readable storage medium, including instructions, which, when run on a computer, enables the computer to execute the above-mentioned unmanned vehicle positioning estimation method that integrates passive observation information.
[0086] The advantages and positive effects of the present invention are:
[0087] The present invention overcomes many problems existing in traditional technologies by effectively fusing the positioning state parameters of the unmanned vehicle itself with the observation values of the relevant observation points and the uncertainty matrix. Specifically, this method can significantly solve the problem of unnecessary computational overhead of unmanned vehicles caused by the inaccuracy of observation point information. In addition, it can also effectively deal with the divergence problem of nonlinear solvers in the process of self-positioning, thereby improving the positioning accuracy and reliability of unmanned vehicles. Through this fusion technology, unmanned vehicles can be more accurately positioned in complex traffic environments, thereby achieving safer and more efficient autonomous driving. BRIEF DESCRIPTION OF THE DRAWINGS
[0088] Figure 1 Flowchart for selecting positioning method for unmanned vehicles;
[0089] Figure 2 Schematic diagram of the two-dimensional scene where the unmanned vehicle and the observation point are located;
[0090] Figure 3 The degree of uncertainty of the unmanned vehicle itself and the degree of uncertainty of the observations;
[0091] Figure 4 Schematic diagram of collaborative calculation of uncertainty between unmanned vehicles and observations;
[0092] Figure 5 Schematic diagram of the degree of uncertainty after cooperative positioning between an unmanned vehicle and an observation point. DETAILED DESCRIPTION
[0093] In order to further understand the content, features and effects of the present invention, the following embodiments are given as examples and described in detail with reference to the accompanying drawings.
[0094] See also Figures 1 to 5 , the specific plan is:
[0095] A method for estimating the location of an unmanned vehicle by integrating passive observation information comprises the following steps:
[0096] The first step is to obtain the positioning status parameters of the unmanned vehicle itself.
[0097] The vehicle's own positioning state parameters include the vehicle's own posture P1 at the current moment; the uncertainty matrix S1 describing the self-positioning of the unmanned vehicle at the current moment. In the scenario of this patent, the environment in which the unmanned vehicle is located is simplified to a two-dimensional environment, namely P1 (x1, y1, α1), where x1 and y1 are the coordinate values in the positioning scenario, and α1 is the heading angle.
[0098] The second step is to obtain the observations of the unmanned vehicle from the relevant observation points (other observation points) in the same system, as well as the uncertainty matrix S2 to S3 of each observation point for the positioning of the unmanned vehicle. n+1 (n is the total number of specific observations).
[0099] The positioning method of the observation point to the unmanned vehicle is defined as the Euclidean distance d between the two and the relative angle β of the observation. Therefore, the coordinates of the unmanned vehicle can be expressed by the following formula:
[0100]
[0101] Where: is the value of the x-coordinate inferred by the i-th observation point based on its own positioning point and observation value; is the y coordinate value inferred by the i-th observation point based on its own positioning point and observation value; x i ,y i , α i is the coordinate system positioning information of the i-th observation point itself; d (i1) is the Euclidean distance between the i-th observation point and the unmanned vehicle; β (i1) is the observation angle of the i-th observation point to the unmanned vehicle.
[0102] Therefore, the Jacobian matrix of the above formula can be obtained:
[0103]
[0104] Where: γ=α i +β (i1) ; γ represents the sum of the observation angle of the i-th observation point relative to the unmanned vehicle and the self-positioning angle of the unmanned vehicle.
[0105] According to the transitivity of the uncertainty matrix, the uncertainty matrix S of the observation of the unmanned vehicle by the nth robot is i+1 It can be expressed by the following formula:
[0106]
[0107] The third step is to calculate the degree of cooperation C2 to C3 between the observation point of the unmanned vehicle and the uncertainty matrix of the positioning of the unmanned vehicle and the uncertainty of the positioning of the unmanned vehicle itself. n+1
[0108] The higher the degree of cooperation between the two, the higher the similarity of the positioning posture of the unmanned vehicle between the two parties. Therefore, the cooperative positioning using the observation value can improve the positioning accuracy of the unmanned vehicle itself. minSet to the minimum collaborative threshold for cooperative positioning. The S1 matrix of the positioning uncertainty of the unmanned vehicle itself can be represented by its position uncertainty ellipse. Therefore, M1 is the area of the position uncertainty ellipse of the unmanned vehicle itself, and M i is the uncertainty ellipse of the pose estimation of the unmanned vehicle at the i-th measurement point, M 1i is the intersection area of the two. In this patent, the positioning collaboration C of the two is defined i+1 .
[0109] Therefore, C i+1 for:
[0110]
[0111] The fourth step is to set the observational cooperation degree of the unmanned vehicle to be greater than the set cooperation threshold C. min The observations are substituted into the nonlinear optimization equations of the unmanned vehicle and the observation points to obtain the final positioning result.
[0112] Assume that the degree of cooperation between the Nth observation point and the unmanned vehicle is greater than the set threshold C min , so the two cooperate to locate. The relative observation is used as the observation value of the nonlinear optimization solver, and the pose calculated by each positioning algorithm is used as the predicted value. Therefore, the predicted pose is:
[0113]
[0114] In the formula, P1, P i+1 is the predicted pose of the unmanned vehicle and the i-th observation point, is the matrix of both.
[0115] Uncertainty matrix of predicted pose:
[0116]
[0117] At this time, the observation equation of the two is based on relative observation values:
[0118]
[0119] Right now:
[0120]
[0121] Where: h() is the observation equation of the two, t is the relative observation noise, R 1i is the uncertainty matrix of noise t
[0122] Therefore, the Jacobian matrix of the observation equation can be obtained:
[0123]
[0124] Where: dx 1i+1 =x i+1 -x1;dy 1i+1 =y i+1 -y1;
[0125] The gain of the nonlinear solver can be obtained as:
[0126]
[0127] Therefore, the pose under cooperative positioning is:
[0128]
[0129] Where:
[0130]
[0131] The uncertainty matrix of the pose is updated as:
[0132]
[0133] Finally, P1 and P can be obtained by calculation i+1 .
[0134] The fifth step is to package the algorithm proposed in this patent into a computer program for real vehicle deployment.
[0135] The deployment environment of the algorithm is an unmanned mining truck equipped with a computer-readable storage medium and related sensors;
[0136] The computer in 1) is connected to the positioning state information of the unmanned vehicle itself and the observation information of the observation point on it;
[0137] The computer in 1) runs the computer program of the positioning estimation method proposed in this patent;
[0138] The results obtained in 4) are applied to other modules of the unmanned vehicle.
[0139] A system for estimating the positioning of an unmanned vehicle by integrating passive observation information, used to implement the method of the above embodiment, the system comprising:
[0140] The first parameter acquisition module, the unmanned vehicle parameter acquisition module, obtains the unmanned vehicle's own positioning state parameters;
[0141] The second parameter acquisition module: obtains the observation value of the observation point for the unmanned vehicle and the uncertainty matrix describing its own observation.
[0142] Calculation module: used to calculate the uncertainty degree of the unmanned vehicle and the uncertainty degree of the observation point for its observation value, and to determine whether to coordinate the observation value of the observation point with the unmanned vehicle.
[0143] Estimation module: combines the positioning information of the unmanned vehicle itself and the positioning information of the observation point to obtain the optimized positioning information of the two.
[0144] The first parameter acquisition module: the self-positioning state parameters of the unmanned vehicle: the vehicle's own posture P1 at the current moment; the uncertainty matrix S1 describing the self-positioning of the unmanned vehicle at the current moment. In the scenario of this patent, the environment in which the unmanned vehicle is located is simplified to a two-dimensional environment, that is, P1 (x1, y1, α1), where x1 and y1 are the coordinate values in the positioning scenario, and α1 is the heading angle.
[0145] The second parameter acquisition module: obtains the observations of the unmanned vehicle from other observation points in the same system, as well as the uncertainty matrix S2 to S3 of each observation point for the positioning of the unmanned vehicle. n (n is the total number of specific observations).
[0146] The positioning method of the observation point to the unmanned vehicle is the Euclidean distance d between the two and the relative angle β of the observation. Therefore, the coordinates of the unmanned vehicle can be expressed by the following formula:
[0147]
[0148] Where: i is a natural number from 1 to n, is the value of the x-coordinate inferred by the n-th observation point for the unmanned vehicle based on its own positioning point and observation value; is the y-coordinate value inferred by the nth observation point based on its own positioning point and observation value; x i ,y i , α i is the coordinate system positioning information of the i-th observation point itself; d (i1) is the Euclidean distance between the i-th observation point and the unmanned vehicle; β (i1) is the observation angle of the i-th observation point to the unmanned vehicle.
[0149] Therefore, the Jacobian matrix of the above formula can be obtained:
[0150]
[0151] Where: γ=α i +β (i1) ; γ represents the sum of the observation angle of the i-th observation point relative to the unmanned vehicle and the self-positioning angle of the unmanned vehicle.
[0152] According to the transitivity of the uncertainty matrix, the uncertainty matrix S of the observation of the unmanned vehicle by the i-th robot is i+1 It can be expressed by the following formula:
[0153]
[0154] Calculation module: used to calculate the uncertainty degree of the unmanned vehicle and the uncertainty degree of the observation point for its observation value, and to determine whether to cooperate with the observation value of the observation point to locate the unmanned vehicle.
[0155] The higher the degree of cooperation between the two, the higher the similarity of the positioning posture of the unmanned vehicle between the two parties. Therefore, the cooperative positioning using the observation value can improve the positioning accuracy of the unmanned vehicle itself. min Set to the minimum collaborative threshold for cooperative positioning. The degree of uncertainty in the positioning of the unmanned vehicle itself, S1, can be represented by its position uncertainty ellipse. Therefore, M1 is the area of the unmanned vehicle's own position uncertainty ellipse, Mi is the position uncertainty ellipse of the unmanned vehicle at the i-th measurement point, and M 1i is the intersection area of the two. In this patent, the positioning collaboration C of the two is defined i+1 .
[0156] Therefore, C i+1 for:
[0157]
[0158] Estimation module: combines the positioning information of the unmanned vehicle itself and the positioning information of the observation point to obtain the optimized positioning information of the two.
[0159] Assume that the degree of cooperation between the i-th observation point and the unmanned vehicle is greater than the set threshold C min , so the two cooperate to locate. The relative observation is used as the observation value of the nonlinear optimization solver, and the pose calculated by each positioning algorithm is used as the predicted value. Therefore, the predicted pose is:
[0160]
[0161] In the formula, P1, P i+1 is the predicted pose of the unmanned vehicle and the i-th observation point, is the matrix of both.
[0162] Uncertainty matrix of predicted pose:
[0163]
[0164] At this time, the observation equation of the two is based on relative observation values:
[0165]
[0166] Right now:
[0167]
[0168] Where: h() is the observation equation of the two, t is the relative observation noise, R 1i is the uncertainty matrix of noise t
[0169] Therefore, the Jacobian matrix of the observation equation can be obtained:
[0170]
[0171] Where: dx 1i+1 =x i+1 -x1;dy 1i+1 =y i+1 -y1;
[0172] The gain of the nonlinear solver can be obtained as:
[0173]
[0174] Therefore, the pose under cooperative positioning is:
[0175]
[0176] Where:
[0177]
[0178] The uncertainty matrix of the pose is updated as:
[0179]
[0180] Finally, P1 and P can be obtained by calculation i+1 .
[0181] An information data processing terminal for an unmanned vehicle positioning estimation method integrating passive observation information.
[0182] A computer-readable storage medium includes instructions, which, when executed on a computer, enable the computer to execute the above-mentioned unmanned vehicle positioning estimation method integrating passive observation information.
[0183] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware or any combination thereof. When the use is implemented in whole or in part in the form of a computer program product, the computer program product includes one or more computer instructions. When the computer program instructions are loaded or executed on a computer, the process or function described in the embodiment of the present invention is generated in whole or in part. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions may be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions may be transmitted from one website site, computer, server or data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (DSL) or wireless (e.g., infrared, wireless, microwave, etc.) mode) to another website site, computer, server or data center. The computer-readable storage medium may be any available medium that a computer can access or a data storage device such as a server or data center that includes one or more available media integrated. The available medium may be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid-state hard disk SolidState Disk (SSD)), etc.
[0184] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Any simple modification, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention are within the scope of the technical solution of the present invention.
Claims
1. A method for estimating the positioning of an unmanned vehicle by integrating passive observation information, characterized in that: include: Obtain the positioning status parameters of the unmanned vehicle itself; The positioning state parameters include: the current position and posture of the unmanned vehicle itself P1 (x1, y1, α1), and the uncertainty matrix S1 describing the positioning of the unmanned vehicle itself at the current moment, wherein x1 and y1 are the horizontal and vertical coordinates in the positioning scene, and α1 is the heading angle; Obtain the observation values of the unmanned vehicle from the relevant observation points and the uncertainty matrix of the positioning of the unmanned vehicle from the n observation points (S2···S n+1 ), where: n is the number of relevant observation points, and n is a natural number greater than 0; Based on S1 and (S2...S n+1 ) Calculate the degree of collaboration (C2···C n+1 ); The degree of cooperation is greater than the cooperation threshold C min The observations are substituted into the nonlinear optimization equation of the unmanned vehicle to obtain the final positioning result.
2. The method for unmanned vehicle positioning estimation integrating passive observation information according to claim 1, characterized in that: The observation values of the unmanned vehicle at the relevant observation points are obtained, and the uncertainty matrix of the positioning of the unmanned vehicle at the n observation points (S2···S n+1 )include: Definition of related observation points The positioning method of the unmanned vehicle is the Euclidean distance d between the two and the relative angle β of the observation. The coordinates of the unmanned vehicle are expressed by the following formula: Where: i is a natural number from 1 to n, is the value of the x-coordinate inferred by the i-th observation point based on its own positioning point and observation value; is the y coordinate value inferred by the i-th observation point based on its own positioning point and observation value; x i ,y i , α i is the coordinate system positioning information of the i-th observation point itself; d (i1) is the Euclidean distance between the i-th observation point and the unmanned vehicle; β (i1) is the observation angle of the i-th observation point to the unmanned vehicle; To find the Jacobian matrix for the above formula, we can get: Where: γ=α i +β (i1) ; γ represents the sum of the observation angle of the ith observation point relative to the unmanned vehicle and the self-positioning angle of the unmanned vehicle; According to the transitivity of the uncertainty matrix, the uncertainty matrix S of the observation quantity of the unmanned vehicle at the i-th related observation point is i+1 It can be expressed by the following formula:
3. The method for unmanned vehicle positioning estimation integrating passive observation information according to claim 2, characterized in that: Based on the S1 and (S2...S n+1 ) Calculate the degree of collaboration (C2···C n+1 ) includes: S1 and (S2···S n+1 ), the higher the degree of cooperation between the two parties, the higher the similarity of the positioning posture of the unmanned vehicle. min Set as the minimum collaborative threshold for cooperative positioning; the S1 matrix of the positioning uncertainty of the unmanned vehicle itself is represented by its position uncertainty ellipse, M1 is the area of the position uncertainty ellipse of the unmanned vehicle itself, and M i+1 is the uncertainty ellipse of the pose estimation of the unmanned vehicle for the i-th related measurement point, M 1i is the intersection area of the two, and defines the positioning collaboration C between the two i+1 , C i+1 It is expressed as:
4. The method for unmanned vehicle positioning estimation integrating passive observation information according to claim 3, characterized in that: The degree of cooperation is greater than the cooperation threshold C min Substitute the observations into the nonlinear optimization equation of the unmanned vehicle, and the final positioning results include: Assuming that the degree of cooperation between the i-th related observation point and the unmanned vehicle is greater than the set threshold C min , the two cooperate to locate, with relative observation as the observation value of the nonlinear optimization solver, and the pose calculated by their own positioning algorithm as the predicted value. The predicted pose is: In the formula, P1, P i+1 is the predicted position of the unmanned vehicle and the i-th related observation point, is the matrix of the two; Uncertainty matrix of predicted pose: At this time, the observation equation of the two is based on relative observation values: Right now: Where: h() is the observation equation of the two, t is the relative observation noise, R 1i is the uncertainty matrix of noise t; The Jacobian matrix of the observation equation is obtained: where: dx 1i+1 = x i+1 - x1; dy 1i+1 = y i+1 - y1; The gain of the nonlinear solver is found to be: The pose under cooperative positioning is: Where: The uncertainty matrix of the pose is updated as: Finally, P1 and P can be obtained by calculation i+1 .
5. An unmanned vehicle positioning estimation system integrating passive observation information, characterized in that: include: The first parameter acquisition module acquires the positioning state parameters of the unmanned vehicle itself; The positioning state parameters include: the current position and posture of the unmanned vehicle itself P1 (x1, y1, α1), and the uncertainty matrix S1 describing the positioning of the unmanned vehicle itself at the current moment, wherein x1 and y1 are the horizontal and vertical coordinates in the positioning scene, and α1 is the heading angle; The second parameter acquisition module obtains the observation amount of the unmanned vehicle from the relevant observation points and the uncertainty matrix of the positioning of the unmanned vehicle from the n observation points (S2···S n+1 ), where: n is the number of relevant observation points, and n is a natural number greater than 0; The calculation module is based on S1 and (S2···S n+1 ) Calculate the degree of collaboration (C2···C n+1 ); The estimation module calculates the degree of cooperation to be greater than the cooperation threshold C min The observations are substituted into the nonlinear optimization equation of the unmanned vehicle to obtain the final positioning result.
6. The unmanned vehicle positioning estimation system integrating passive observation information according to claim 5, characterized in that: The second parameter acquisition module includes: Definition of related observation points The positioning method of the unmanned vehicle is the Euclidean distance d between the two and the relative angle β of the observation. The coordinates of the unmanned vehicle are expressed by the following formula: Where: i is a natural number between 1 and n, is the value of the x-coordinate inferred by the i-th observation point based on its own positioning point and observation value; is the y coordinate value inferred by the i-th observation point based on its own positioning point and observation value; x i ,y i , α i is the coordinate system positioning information of the i-th observation point itself; d (i1) is the Euclidean distance between the i-th observation point and the unmanned vehicle; β (i1) is the observation angle of the i-th observation point to the unmanned vehicle; To find the Jacobian matrix for the above formula, we can get: Where: γ=α i +β (i1) ; γ represents the sum of the observation angle of the ith observation point relative to the unmanned vehicle and the self-positioning angle of the unmanned vehicle; According to the transitivity of the uncertainty matrix, the uncertainty matrix S of the observation quantity of the unmanned vehicle at the i-th related observation point is i+1 It can be expressed by the following formula:
7. The method for estimating the position of an unmanned vehicle by integrating passive observation information according to claim 6, characterized in that: The calculation module includes: S1 and (S2...S n+1 ), the higher the degree of cooperation between the two parties, the higher the similarity of the positioning posture of the unmanned vehicle. min Set as the minimum collaborative threshold for cooperative positioning; the S1 matrix of the positioning uncertainty of the unmanned vehicle itself is represented by its position uncertainty ellipse, M1 is the area of the position uncertainty ellipse of the unmanned vehicle itself, and M i is the uncertainty ellipse of the pose estimation of the unmanned vehicle for the i-th related measurement point, M 1i is the intersection area of the two, and defines the positioning collaboration C between the two i+1 , C i+1 It is expressed as:
8. The method for unmanned vehicle positioning estimation integrating passive observation information according to claim 7, characterized in that: The estimation module includes: assuming that the degree of cooperation between the i-th related observation point and the unmanned vehicle is greater than a set threshold C min , the two cooperate to locate, with relative observation as the observation value of the nonlinear optimization solver, and the pose calculated by their own positioning algorithm as the predicted value. The predicted pose is: In the formula, P1, P i+1 is the predicted pose of the unmanned vehicle and the Nth related observation point, is the matrix of the two; Uncertainty matrix for predicted pose: At this time, the observation equation of the two is based on relative observation values: Right now: Where: h() is the observation equation of the two, t is the relative observation noise, R 1i is the uncertainty matrix of noise t; The Jacobian matrix of the observation equation is obtained: where: dx 1i+1 = x i+1 - x1; dy 1i+1 = y i+1 - y1; The gain of the nonlinear solver is found to be: The pose under cooperative positioning is: Where: The uncertainty matrix of the pose is updated as: Finally, P1 and P can be obtained by calculation i+1 .
9. A computer program product, comprising a computer program, characterized in that The computer program is executed by a processor to provide a positioning estimation method integrating passive observation information as described in any one of claims 1 to 4.
10. A computer-readable storage medium comprising instructions, which, when executed on a computer, enables the computer to execute the positioning estimation method for fusing passive observation information as described in any one of claims 1 to 4.