Binocular positioning method using known light source arrangement

Abstract

The application provides a binocular positioning method using a known light source arrangement, and belongs to the field of computer vision positioning. The application captures the positions of three visible light feature points of a target object in a camera pixel image during movement of a binocular camera, and an inclination angle and a roll angle obtained by a six-axis inertial sensor. A unscented Kalman filter is used to integrate data of the inertial sensor and data of the binocular camera, so that visible light positioning is realized, that is, the position of a visible light feature point of the target object in a three-dimensional space and a heading angle are accurately calculated. The application has wide applicability, good stability, and can be used for positioning at a farther distance. The stability is excellent not only in an indoor environment, but also in complex outdoor environments such as urban blocks, mountains and oceans.

Description

Technical Field

[0001] This invention belongs to the field of computer vision positioning. It is a method for positioning using images captured by a binocular camera and data from a six-axis inertial sensor. This method can determine the position and orientation of a target object by using visible light feature points in the digital images from the two cameras and data from the inertial sensor, thereby achieving object positioning or navigation. Background Technology

[0002] Localization and navigation often rely on inertial sensors and stereo cameras, which provide information about motion and space by reading inertial forces and optical images, respectively. However, each type of sensor has its drawbacks: for example, inertial sensors can be affected by noise and drift, while stereo cameras may produce pixel errors in visible light feature point recognition due to calibration accuracy and lighting conditions. Therefore, in localization algorithms, we often integrate data from both sensors to improve stability and accuracy. However, this method requires high-precision raw data from inertial sensors or the three attitude angles of the target object, demanding high data accuracy and may even require the addition of a geomagnetic sensor, which imposes significant limitations on environmental deployment.

[0003] Based on this, this invention proposes a novel algorithm. When we know the arrangement of visible light feature points of the target object and their positions on the binocular camera image, this algorithm can combine low-precision inertial sensor and binocular camera image data to calculate the pose of the target object. This calculation method is stable and accurate, capable of long-distance positioning, and can still accurately calculate position and orientation even in the presence of noise interference during long-distance feature point recognition.

[0004] In the patent "2023104914792 A Binocular Positioning Method with Unconstrained Light Source Arrangement" (hereinafter referred to as Patent 1) applied for by our research group, an algorithm for locating three visible light feature points using a binocular camera and an inertial sensor was also proposed. This patent is aimed at a target object with three unconstrained visible light feature points, that is, the relative positions of the three visible light feature points are unknown. The algorithm has a certain accuracy and robustness, but in application scenarios where the arrangement of the three visible light feature points is known, it cannot utilize this arrangement information, and the accuracy needs to be further improved.

[0005] The difference between this invention and Patent 1 is that this invention can use the positional arrangement information of visible light feature points to calculate the position of only one visible light feature point to locate the target object, thus having higher pose calculation accuracy, stronger robustness, and longer positioning distance than Patent 1. Summary of the Invention

[0006] This invention aims to address the problems existing in the binocular camera and inertial sensor fusion positioning algorithm, namely, low positioning accuracy and large attitude angle errors due to insufficient precision of the inertial sensor or noise in feature point recognition. This invention specifically targets target objects with three visible light feature points of known relative positions. By capturing the positions of the three visible light feature points of the target object in the camera pixel image during movement using a binocular camera, and obtaining the pitch and roll angles from a six-axis inertial sensor, we can calculate the position and heading angle of one visible light feature point of the target object in three-dimensional space.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0008] A binocular localization method utilizing a known light source layout is disclosed. This method integrates data from inertial sensors and binocular cameras using an unscented Kalman filter to achieve visible light localization. First, within the framework of the unscented Kalman filter algorithm, a constant velocity model (i.e., the linear and angular velocities of the target object remain constant across two consecutive moments) is used to establish the process equations for predicting the target object's position and attitude angles in three-dimensional space. Next, a pinhole projection model of the binocular camera is used to construct the measurement equations, thereby correcting the parts of the process equations that calculate the coordinates of the target object's visible light feature points and heading angles. Notably, the visible light feature point positions and attitude angles calculated by this invention are robust to noise from both the six-axis inertial sensor data and the noise from the binocular camera's feature point recognition, while maintaining high accuracy and stability even when the target object is far from the camera.

[0009] The measurement specifically includes the following steps:

[0010] 1. Initialization

[0011] 1.1 Camera Calibration

[0012] Let the two cameras of the stereo camera be Camera 1 and Camera 2, respectively. Perform stereo camera calibration on Camera 1 and Camera 2 to obtain the intrinsic parameters (focal length: f, pixel image center point: (u0, v0), pixel size: dx, dy) and extrinsic parameters (rotation matrix of Camera 2 relative to Camera 1: R) of each camera. CCS (CCS: Camera Coordinate System), Translation matrix: T CCS Here, the intrinsic parameters of camera 1 and camera 2 are the same, and these parameters will be used in the following camera measurement equations;

[0013] 1.2 Establishing the State Vector of the Unscented Kalman Filter

[0014] Establish a right-handed three-dimensional coordinate system with the optical center of camera 1's lens as the origin and the outward direction of the lens as the positive z-axis. This coordinate system is called Camera Coordinate System 1 (CCS1). Similarly, establish Camera Coordinate System 2 (CCS2) on camera 2. Let CCS1 be the principal coordinate system, P i,k =(x i,k ,y i,k ,z i,k ) T (i = 1, 2, 3) are the coordinates of the three visible light feature points of the target object at time k in CCS1.

[0015] Let the state vector (hereinafter referred to as the state) of the k-th cycle (hereinafter referred to as time k) be χ. k =[P 1,k T k β k ] T Among them, P 1,k Let T be the coordinates of a visible light feature point in the motion of the target object at time k in CCS1 (let the coordinates of this visible light feature point be equivalent to the coordinates of the target object, hereinafter referred to as: target object coordinates); k Let β be the translation vector at time k; k The heading angle is at time k. The initial state is established as the first step in using the unscented Kalman filter, where k = 0 represents the initial state of the unscented Kalman filter, which can be set based on the current distance and relative attitude between the stereo camera and the target object.

[0016] 1.3 Obtaining the relative positions of three visible light feature points

[0017] This invention calculates the coordinates and heading angle of a target object given the relative positions of three visible light feature points. This requires first obtaining the relative positions of the three visible light feature points. First, establish... Figure 2 The target object coordinate system is shown. An Object Coordinate System (OCS) is established with the first visible light feature point as the origin, parallel to the axes of CCS1. The position offsets of the remaining two feature points relative to the first feature point are obtained as t. 12 and t 13 The offsets, representing the positional shifts of the second and third visible light feature points relative to the first visible light feature point under OCS, are:

[0018]

[0019] P1 OCS The coordinates of the first visible light feature point under OCS. and These are the coordinates of the second and third visible light feature points in the OCS.

[0020] 1.4 Establishing the process equations for an unscented Kalman filter

[0021] First, according to the constant velocity model, the translation vector and heading angle remain unchanged in two adjacent calculation cycles, thus allowing the state vector χ at time k to be determined. k (Hereinafter referred to as state) establishes the following equation:

[0022]

[0023] The left side of the equation represents the state at time k; the right side is the equation for calculating the state at time k using the state at time k-1. P 1,k-1 Let T be the coordinates of the target object in three-dimensional space under CCS1 at time k-1; k-1 Let R be the translation vector at time k-1; k Let be the rotation matrix at time k, and let be the pitch angle α at time k. k and roll angle γ k The pitch angle at time k-1 is calculated using the following formula:

[0024]

[0025] This formula is the Euler-Rodrigues formula; w k This is the noise of the predicted state at time k, which is automatically estimated by the unscented Kalman filter algorithm.

[0026] The above equations are collectively called process equations, written as χ. k =F(χ) k-1 ,α k ,γ k The process equations are established as the second step in using an unscented Kalman filter. These equations describe how the state χ at time k-1 is derived. k-1 Predict the state χ at time k k ;

[0027] 1.5 Establishing the Measurement Equations for the Unscented Kalman Filter

[0028] A camera images any point in 3D space onto its pixel image. If we can obtain the positional relationship between the coordinates of that point in 3D space and the camera's pixel image, then given the coordinates of that point on the camera's pixel image, we can obtain partial information about the point's coordinates in 3D space. This information can be used with the Kalman filter algorithm to analyze the predicted state χ in section 1.2. kThe process of correction is called measurement. (The relationship between the coordinates of a point in three-dimensional space and the coordinates of a camera pixel image is unidirectional. Knowing the coordinates in three-dimensional space, the coordinates of the camera pixel image can be calculated, but the reverse is not possible, because one point in the camera pixel image corresponds to countless points on a straight line in three-dimensional space.)

[0029] The stereo camera has already been calibrated in section 1.1. Let:

[0030]

[0031] This matrix A is called the intrinsic parameter matrix.

[0032] For time k, the target object coordinates under CCS1 are: P 1,k It can be changed to an augmented form: P 1,k =(x 1,k ,y 1,k ,z 1,k ,1) T The pixel coordinates of this point in the image of camera 1 (The superscript p1 represents the pixel image from camera 1). P represents the x and y coordinates of the point in the pixel image; 1,k and The relationship is:

[0033]

[0034] This equation is hereby denoted as: E3 is a third-order identity matrix. Additionally, P needs to be obtained. 1,k Coordinates on the pixel image of camera 2 (The superscript p2 represents the pixel image from camera 2), first calculate P. 1,k Coordinates in CCS2 The coordinates below are (the superscript CCS2 refers to the coordinates in camera coordinate system 2):

[0035]

[0036] but for:

[0037]

[0038] The formula is written as:

[0039] Finally, the equation and Merge into:

[0040]

[0041] This equation represents the pinhole projection model of a binocular camera for P. 1,k The measurement equation, the measurement equation will determine the coordinates of point P under CCS1. 1,k The pixel image coordinates of camera 1 and camera 2 in the stereo camera system are converted into the predicted state χ from 1.2. k P in 1,k Calculate the pixel image coordinates of camera 1 and camera 2. Obtain from the stereo cameras. And obtain R from the binocular camera calibration results CCS T CCS Then, an unscented Kalman filter can be used to analyze the P state. 1,k Corrections are made. Meanwhile, in this invention, point P1 under the OCS is known. OCS , and Given the relative positions, P can be used simultaneously under this observation model. 2,k P 3,k For P 1,k Observations were conducted (detailed methods are shown in Section 2.6).

[0042] 1.6 Initialization Parameters of Unscented Kalman Filter

[0043] In sections 1.2, 1.4, and 1.5, the Kalman filters for the state, process, and measurement equations were established. It is also necessary to determine the initial values ​​for the unscented Kalman filter, which are the initial position and attitude of the target object. Furthermore, the process noise covariance matrix S needs to be set to ensure the accuracy of the process equations in describing the changes in the coordinates of the three feature points and the attitude angles of the actual target object during its motion. Correspondingly, the measurement noise covariance matrix L needs to be set to ensure the accuracy of the measurement equations in describing the camera imaging.

[0044] 2. Iteration of Unscented Kalman Filter

[0045] 2.1 Acquisition of Pitch and Roll Angles

[0046] For a target object with three unconstrained visible light feature points, a six-axis inertial sensor is installed to obtain three real-time changing attitude angles during its motion in three-dimensional space: pitch angle, yaw angle, and roll angle. These three attitude angles at any given time k are denoted as α. k ,β k γ k See the attached diagram description. Figure 2 For a six-axis inertial sensor, the accurate pitch and roll angles of the target object can be obtained, but the heading angle generally has a large cumulative error. Therefore, this invention only uses the obtained pitch and roll angles, and simultaneously calculates the heading angle without cumulative error.

[0047] 2.2 Image Acquisition

[0048] For a target object with three unconstrained visible light feature points, a binocular camera is used to capture the positions of the three visible light feature points in the camera pixel image during its motion. The coordinates of the i-th visible light feature point at any time k in the pixel image of the j-th camera are given by: Image coordinates (superscript pj represents the pixel image of the j-th camera).

[0049] 2.3 Calculate the sigma point set of the state at time k-1

[0050] The optimal state X at time k-1 k-1 The states are distributed into 2n+1 sigma point sets according to the following rules:

[0051]

[0052] χ l,k-1 (l=0,1,...,2n) is the optimal state X at time k-1. k-1 The sigma point set, i.e., the sigma point set of the state at time k-1; n is the sigma point set of X. k-1 The number of members; λ is the diagonal factor, a constant, calculated as λ = ε. 2 (n+κ)-n, where ε is the distribution range factor and is a constant, and κ is the distribution factor and is a constant; Q k-1 The optimal state covariance matrix at time k-1 is automatically estimated by the unscented Kalman filter.

[0053] 2.4 Calculate the sigma point set of the process at time k

[0054] The process equation F(χ) was established in 1.3. k-1 ,α k ,γ k ,w k-1 ), and the sigma point set χ of the state at time k-1 in 2.3 l,k-1 Substituting (l=0,1,...,2n) into this equation to calculate the update of the sigma point set of the state in the process equation is equivalent to the sigma point set χ of the process at time k. l,k (l=0,1,...,2n):

[0055]

[0056] Among them W l m (l=0,1,...,2n) is the mean weight vector, and the calculation formula is:

[0057]

[0058] Let χ be the sigma point set of the process. l,k (l=0,1,...,2n) lies in the mean weight vector W l m The mean of the sigma point set of a process under (l=0,1,...,2n).

[0059] 2.5 Calculate the state covariance matrix at time k

[0060]

[0061] W is the state covariance matrix at time k; l c (l=0,1,...,2n) is the variance weight vector, and the calculation formula is:

[0062]

[0063] S is the process noise covariance matrix, which is set in 1.5.

[0064] 2.6 Calculate the coordinates of the second and third characteristic points in the sigma point set for the process at time k.

[0065] In section 2.4, the sigma point set χ for the process at time k was calculated. l,k (l=0,1,...,2n), in order to capture the pixel images of the second and third feature points in camera 1 and camera 2 ( and Introducing this into the observation equation also requires calculating the coordinates of the other two visible light feature points in the sigma point set for this process.

[0066] 2.6.1 Calculate the relative position rotation matrix between the second and third feature points.

[0067] make Let be the heading angle of the sigma point set for the process at time k. Let the relative position matrix (hereinafter referred to as the relative position rotation matrix) be the matrix of the two other visible light feature points relative to the first visible light feature point in the sigma point set of the process at time k. Then:

[0068]

[0069] R Xk-1 Let be the relative position rotation matrix of the optimal state at time k-1.

[0070] 2.6.2 Calculate the coordinates of the second and third feature points

[0071] make Let the coordinates of the second and third feature points in the sigma point set at time k be respectively. Then we have:

[0072]

[0073] The coordinates of the first feature point in the process sigma point set at time k.

[0074] t 12 and t 13 This represents the position of the first feature point relative to the second and third feature points under the OCS obtained in 1.3.

[0075] 2.7 Calculate the sigma point set of measurements at time k

[0076] Similarly, the measurement equation H(A,P) was established in 1.4. i,k ,R CCS ,T CCS ), in 2.6 Substitute this into the equation to calculate the update of the point set in the measurement equation (i.e., the sigma point set measured at time k) Z. l,k (l=0,1,...,2n):

[0077]

[0078] Z l,k (l=0,1,...,2n) is the process sigma point set at time k. Let W be the mean weight vector at time k. l m The sigma point set mean of a process under (l=0,1,...,2n).

[0079] 2.8 Calculate the measurement covariance matrix at time k

[0080]

[0081] Let L be the measurement covariance matrix at time k; L is the measurement noise covariance matrix, which is set in 1.5.

[0082] 2.9 Calculate the Kalman gain at time k

[0083]

[0084]

[0085] Let K be the state-measurement joint covariance matrix at time k; k Let K be the Kalman gain at time k.

[0086] 2.10 Calculate the optimal state at time k and repeat the iterations.

[0087]

[0088] X k The optimal state at time k; z k The coordinates of the pixel image of the visible light feature points at time k obtained from 2.2. P k Let be the covariance matrix of the optimal state at time k.

[0089] 2.11 Calculate the relative position rotation matrix of the optimal state at time k.

[0090] Using X in 2.9 k Calculate the relative position rotation matrix of the optimal state at time k:

[0091]

[0092] α k and γ k The pitch and roll angles at time k obtained in section 2.1, β k This represents the heading angle under optimal conditions.

[0093] 2.12 Output and Repeated Iterations

[0094] After calculating the optimal state and optimal state covariance at time k in section 2.10, the iteration at time k is complete, X k This refers to the coordinates and attitude angles of the target object under CCS1. Let this time k be time k-1, and the next time be time k. Return to 2.1 to recalculate the optimal state and the optimal state covariance at time k.

[0095] The effects and benefits of this invention are:

[0096] (1) Wide applicability: The advantage of this invention lies in its strong applicability. It only requires obtaining the pitch and roll angles of the target object to deal with the noise of the inertial sensor, and is therefore insensitive to it. The iterative solution algorithm based on the unscented Kalman filter reduces the computational complexity and the computational power requirement, which makes this invention applicable in various environments.

[0097] (2) Excellent stability: Compared with other visible light positioning schemes, this invention has stronger resistance to the recognition error of visible light feature point coordinates in camera pixel images, thus enabling positioning at greater distances. Its stability is not only excellent in indoor environments, but also applicable to complex outdoor environments such as urban blocks, mountains, and oceans. Attached Figure Description

[0098] Figure 1 This is the scenario model for implementing the present invention.

[0099] Figure 2 The coordinate system and three attitude angles of the target object are explained.

[0100] Figure 3 This is a flowchart of the present invention. Detailed Implementation

[0101] The specific embodiments of the present invention are described in detail below with reference to the technical solution (and accompanying drawings).

[0102] Figure 1 This is a scenario model for implementing the present invention. Figure 1 At any time k, camera 1 and camera 2 form a stereo camera, O 1 and O 2 These are the optical centers of the lenses of camera 1 and camera 2, respectively, with O... 1 and O 2 With the origin at the camera and the positive z-axis pointing outwards from the lens, establish two three-dimensional right-handed coordinate systems, namely CCS1 and CCS2; the pixel images of camera 1 and camera 2 lie in the positive z-axis direction of the stereo camera and are correlated with xO. i The z-axis of CCS1 and CCS2 are parallel, and the intersection of their respective pixel images with the z-axis is the center point (u0, v0) of the pixel image. It has three visible light feature points P. i,k The target object (i = 1, 2, 3) moves in three-dimensional space. These three visible light feature points are captured by the binocular camera and imaged onto two pixel images. Their coordinates on the pixel images of camera 1 and camera 2 are P. i pj (i = 1, 2, 3; j = 1, 2).

[0103] Figure 2 The coordinate system and three attitude angles of the target object are explained below. The target object coordinate system (OCS) is established with the center of mass of the target object as the origin, parallel to the axes of CCS1. The three attitude angles include pitch (α), yaw (β), and roll (γ), which are the angles of rotation of the target object around the z-axis, y-axis, and x-axis of the OCS, respectively.

[0104] Figure 3 The flowchart of this invention illustrates the specific steps of implementing this patented invention in a real-world river boat speed measurement scenario:

[0105] Step 1: Select a survey vessel already placed in the river, equipped with three visible light feature points and a six-axis inertial sensor. Establish the unscented Kalman filter state vector, process equations, and measurement equations for this survey vessel and its dynamic behavior.

[0106] Step 2: Place a binocular camera on the riverbank and calibrate it beforehand to obtain the binocular camera's extrinsic and intrinsic parameters.

[0107] Step 3: Initialize the parameters of the unscented Kalman filter.

[0108] Step 4: Use a binocular camera to measure the relative positions of three visible light feature points on the ship.

[0109] Step 5: For each measurement cycle, pitch and roll angle data are acquired using the six-axis inertial sensor on the survey vessel, while the pixel image positions of three visible light feature points on camera 1 and camera 2 are acquired using a binocular camera.

[0110] Step 6: Based on the acquired data, calculate the optimal state sigma point set of the previous measurement cycle, the sigma point set during the calculation process, and the covariance matrix under the calculated state.

[0111] Step 7: Calculate the relative position rotation matrix under the process sigma point set, and calculate the coordinates of the second and third visible light feature points.

[0112] Step 8: Calculate the measurement sigma point set, calculate the measurement covariance matrix, calculate the Kalman gain, the optimal state, and the optimal state covariance matrix.

[0113] Step 9: Calculate the relative position rotation matrix under optimal conditions, and output the position and heading angle of the survey vessel.

[0114] Step 10: Set the current measurement cycle as the previous measurement cycle, set the next measurement cycle as the current measurement cycle, and then return to step 5 to calculate the current measurement cycle again.

[0115] After verification, the algorithm has been shown to maintain a positioning error of no more than 1.5m and an attitude angle error of no more than 5° in river boat speed measurement scenarios, even when the camera is more than 120 meters away from the boat.

[0116] The above-described embodiments are merely illustrative of the implementation methods of the present invention, but should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the protection scope of the present invention.

Claims

1. A binocular positioning method utilizing a known light source arrangement, targeting a object with three visible light feature points of known relative positions, characterized in that... By capturing the positions of three visible light feature points of the target object in the camera pixel image during motion using a binocular camera, and obtaining the pitch and roll angles from a six-axis inertial sensor, an unscented Kalman filter is used to integrate the data from the inertial sensor and the binocular camera to achieve visible light positioning, that is, to accurately calculate the position and heading angle of a visible light feature point of the target object in three-dimensional space. In the integration process described above: within the framework of the unscented Kalman filter algorithm, a constant velocity model is used to establish the process equation for predicting the position and attitude angle of the target object in three-dimensional space. The constant velocity model means that the linear velocity and angular velocity of the target object remain constant at two consecutive moments. The pinhole projection model of a binocular camera is used to construct the measurement equation, thereby correcting the part of the process equation that calculates the coordinates of the visible light feature points of the target object and the heading angle. Specifically, the following steps are included:

1. Initialization; 1.1 Camera calibration; Let the left and right cameras of the stereo camera be Camera 1 and Camera 2, respectively; perform stereo camera calibration on Camera 1 and Camera 2 to obtain the intrinsic and extrinsic parameters of the two cameras respectively; 1.2 Establish the state vector of the unscented Kalman filter; 1.3 Obtaining the relative positions of three visible light feature points; To calculate the coordinates and heading angle of a target object given the relative positions of three visible light feature points, it is necessary to first obtain the relative positions of the three visible light feature points; then establish the target object's coordinate system; and finally, use the first visible light feature point as the origin, and... CCS1 Establish the target object coordinate system in directions where all axes are parallel. OCS ; Obtain the position offsets of the remaining two feature points relative to the first feature point. and The offsets, representing the positional shifts of the second and third visible light feature points relative to the first visible light feature point under OCS, are: The coordinates of the first visible light feature point under OCS. and These are the coordinates of the second and third visible light feature points in the OCS. 1.4 Establish the process equations for the unscented Kalman filter; First, according to the constant velocity model, the translation vector and heading angle remain unchanged between two adjacent calculation cycles, thus allowing for... k State vector at time step Establish the following equation: The left side of the equation is k The state at time; the right side of the equation is used k-1 State calculation at time step k The equation for the state at any given moment. for k-1 Moment CCS1 The coordinates of the target object in the three-dimensional space where it moves; Recorded as k-1 The translation vector at time; Recorded as k The rotation matrix at time, through k Pitch angle at any moment and roll angle as well as k-1 The pitch angle at time t is calculated using the following formula: Then it is k The noise in the state prediction at any given time is automatically estimated by the unscented Kalman filter algorithm; The above equations are collectively called process equations, written as: The process equation is the second step in using an unscented Kalman filter, and this equation describes how the process equation is derived from the unscented Kalman filter. k-1 Moment State Predicted k state of time ; 1.5 Establish the measurement equations for the unscented Kalman filter; The stereo camera has been calibrated in section 1.1, and the intrinsic parameter matrix has been obtained; For the k At what time, for the target object coordinates in CCS1: Change to an augmented form: The pixel coordinates of this point in the image of camera 1 superscript p1 For the pixel image of camera 1, Let x and y be the coordinates of the point in the pixel image; and The relationship is: This equation is hereby denoted as: ;in It is a third-order identity matrix; at the same time, it is also necessary to obtain Coordinates on the pixel image of camera 2 superscript p2 For the pixel image of camera 2, first calculate exist CCS2 coordinates below superscript CCS2 This indicates that the point has coordinates in camera coordinate system 2: in For the rotation matrix of camera 2 relative to camera 1 in a stereo camera system, Let be the translation matrix of camera 2 relative to camera 1 in a stereo camera setup. for: The matrix Let the intrinsic parameter matrix of the stereo camera be denoted as: ; Finally, the equation and Merge into: This equation represents the pinhole projection model of a binocular camera. The measurement equation, the measurement equation will CCS1 The coordinates of the point below This is converted into the pixel image coordinates of camera 1 and camera 2 in the stereo camera, which is the predicted state from 1.

2. In Calculate the pixel image coordinates of camera 1 and camera 2; Obtained from a stereo camera , And obtained from the binocular camera calibration results , Then, an unscented Kalman filter is used to process the states. Make corrections; at the same time, in the known OCS The point below , and Given the relative positions, this observation model simultaneously uses right Conduct observations; 1.6 Initialization parameters of the unscented Kalman filter; 2. Iteration of the unscented Kalman filter; 2.1 Acquisition of pitch and roll angles; 2.2 Image Acquisition; For a target object with three unconstrained visible light feature points, a binocular camera is used to capture the positions of these three visible light feature points in the camera pixel image during its motion. (The last sentence appears to be incomplete and possibly refers to an unrelated instruction: "arbitrary.") k The first moment i The visible light feature point at the th in the ... j The coordinates of the pixel image from each camera are Image coordinates, where superscript pj For the first j Pixel images from a single camera; 2.3, Calculation k-1 state at any moment sigma Point set; 2.4, Calculation k Time process sigma Point set; 2.5, Calculation k The state covariance matrix at time t; 2.6 Calculate the coordinates of the second and third feature points in the sigma point set for the process at time k; The calculation was performed in 2.

4. k Time process sigma dot set In order to capture the pixel images of the second and third feature points in camera 1 and camera 2 and Introduced into the observation equation, it is also necessary to calculate the coordinates of the other two visible light feature points under the sigma point set in this process; 2.6.1 Calculate the relative position rotation matrix between the second and third feature points; make for k Time process sigma The heading angle of the point set, for k In the process of time sigma The relative position matrices of the other two visible light feature points in the point set relative to the first visible light feature point are: Let be the relative position rotation matrix of the optimal state at time k-1; 2.6.2 Calculate the coordinates of the second and third feature points. make Let the coordinates of the second and third feature points in the sigma point set at time k be respectively. Then we have: Let k be the coordinates of the first feature point in the process sigma point set at time k. and For those obtained in 1.3 OCS The position of the first feature point relative to the second and third feature points; 2.7, Calculation k Measurement of time sigma dot set The measurement equations were established in section 1.

4. , in 2.6 Substitute these values ​​into the equation to calculate the update of the point set in the measurement equation. : for k The process of time sigma Point set, for k At any given time in the mean-weight vector The process of going down sigma Mean of a point set; 2.8, Calculation k The measurement covariance matrix at time; for k The measurement covariance matrix at time; To measure the noise covariance matrix, set it in 1.5; 2.9, Calculation k Kalman gain at time step; for k The joint covariance matrix of state and measurement at time t; for k Kalman gain at time step; 2.10, Calculation k The optimal state at each time step and repeated iterations; for k The optimal state at any given time; Obtained from 2.2 k Coordinates of the visible light feature points in the pixel image at time t. ; for k The covariance matrix of the optimal state at time t; 2.11 Calculate the relative position rotation matrix of the optimal state at time k; Using 2.9 Calculate the relative position rotation matrix of the optimal state at time k: and The pitch and roll angles at time k obtained in section 2.1 are... The heading angle is the optimal angle. 2.12, Output and Repeated Iterations; Calculated in 2.10 k The optimal state at time t and k After the optimal state covariance at time 1, k The iteration of time is complete. That is, the coordinates and attitude angles of the target object under CCS1; let this k Time for k-1 The next moment is k At that moment, return to 2.1 and recalculate. k The optimal state at time t and k The optimal state covariance at time t.

2. The binocular positioning method using a known light source arrangement according to claim 1, characterized in that, The specific steps for establishing the state vector of the unscented Kalman filter in step 1.2 are as follows: With the optical center of camera 1's lens as the origin and the outward direction of the lens as the positive z-axis, establish a right-handed three-dimensional coordinate system. This coordinate system is called camera coordinate system 1. CCS1 Similarly, establish camera coordinate system 2 on camera 2, that is... CCS2 ; make CCS1 Main coordinate system The coordinates of the three visible light feature points of the target object at time k are shown in CCS1. Let the calculation cycle be the first k The state vector for each period is: ;in, For the first k A visible light feature point of the target object at time t is in motion. CCS1 The coordinates below; Recorded as k The translation vector at time; Xiangwei k The heading angle at time; establishing the state as the first step using the unscented Kalman filter, where, when k=0 The initial state of the unscented Kalman filter can be set based on the distance and relative attitude between the current stereo camera and the target object.

3. The binocular positioning method using a known light source arrangement according to claim 1, characterized in that, Steps 2.1, 2.3, 2.4, and 2.5 are as follows: 2.1 Acquisition of pitch and roll angles; For a target object with three unconstrained visible light feature points, a six-axis inertial sensor is installed to obtain three real-time changing attitude angles during its motion in three-dimensional space: pitch angle, yaw angle, and roll angle. An arbitrary third angle is also considered. k The three attitude angles at time t are denoted as follows: , , ; 2.3, Calculation k-1 state at any moment sigma Point set; Will k-1 optimal state at time 1 Distribute according to the following rules 2n+1 each state sigm Point set: Then it is k-1 optimal state at time 1 The sigma point set, i.e. k-1 state at any moment sigma Point set; n for Number of members; is the diagonal factor, and is a constant. The calculation formula is: , The distribution range factor is denoted as a constant. is the distribution factor, and is a constant; for k-1 The optimal state covariance matrix at time t is automatically estimated by the unscented Kalman filter; 2.4, Calculation k Time process sigma Point set; The process equations were established in section 1.

3. , in 2.3 k-1 state at any moment sigma dot set Substitute the equation to calculate the state sigma The update of the point set in the process equation is... k Time process sigma dot set : in The mean-weighted vector is calculated using the following formula: For the process sigma dot set In the mean weight vector The process of going down sigma The mean of a point set; 2.5, Calculation k The state covariance matrix at time t; for k The state covariance matrix at time t; The variance weight vector is calculated using the following formula: The process noise covariance matrix is... This is the distribution range factor, a constant, set in version 1.5.

Images