A terminal device positioning method based on an Euler angle perturbation model

Abstract

The application relates to a terminal device positioning method based on an Euler angle perturbation model, and belongs to the technical field of automatic positioning, and solves the problems of low positioning efficiency and low accuracy of the pose of a terminal device in the prior art. The terminal device positioning method based on the Euler angle perturbation model can improve the acquisition efficiency and accuracy of the pose of the terminal device, is convenient to debug, and can be widely applied to various fields of unmanned aerial vehicles, unmanned driving and robots.

Description

Technical Field

[0001] This invention belongs to the field of automatic positioning technology and relates to a terminal device positioning method based on an Euler angle perturbation model. Background Technology

[0002] With the widespread application and development of various terminal devices (such as robots and drones), autonomous localization technology for these devices is becoming increasingly indispensable. Autonomous localization includes position resolution and attitude resolution of the terminal device, which obtains its pose information by analyzing information measured by the sensors carried by the device itself. Attitude resolution is mostly obtained by inertial sensors. However, in complex and ever-changing real-world environments, relying solely on active sensors such as gyroscopes to acquire attitude information is not robust enough. Therefore, existing methods often incorporate passive sensor information such as vision and laser point clouds to further constrain the cumulative error problem caused by a single odometry.

[0003] Currently, the common approach to pose determination is to utilize information obtained from various sensors to establish constraints and then acquire pose information through least-squares descent iterations. However, due to the redundancy and cumulative nature of rotation matrix parameters, traditional iterative descent methods cannot directly solve for rotational motion using the rotation matrix. Currently, Lie algebras are often used to calculate the Jacobian constraint equations. This method effectively solves the problem of solving for rotational motion through descent iterations, but Lie algebras require exponentiation and antisymmetric operations to update parameters, and lack intuitiveness. Besides using Lie algebras to solve pose, some localization methods based on Euler angles have been proposed. However, these methods typically require complex Jacobian matrices, thus their efficiency is relatively lower than that of Lie algebra methods. Summary of the Invention

[0004] In view of the above problems, the present invention provides a terminal device positioning method based on the Euler angle perturbation model, which can improve the efficiency and accuracy of obtaining the terminal device pose.

[0005] This invention provides an agent localization method based on an Euler angle perturbation model, the specific steps of which are as follows:

[0006] Step 1: The terminal device acquires its own and environmental information data through the posture sensor;

[0007] Step 2: Establish constraint equations based on the obtained information data:

[0008] Source feature points of information data acquired by terminal devices After rotation R∈SO(3) and translation In the new coordinate system, the corresponding coordinates of the source feature points are: The constraint equations are obtained as follows:

[0009] e(θ,t)=||r(θ,t)||2=||T(θ,t)PQ||2=||R(θ)P+tQ||2;

[0010] Where (θ,t) are the pose parameters to be solved for the terminal device, θ are the Euler angles to be solved for the terminal device, and t are the translation amounts to be solved for the terminal device; r(θ,t)=R(θ)P+tQ is the global error equation, R(θ) represents the rotation matrix corresponding to the Euler angle θ; T(θ,t) is the 4-dimensional transformation matrix;

[0011]

[0012] Step 3: Iteratively update the pose parameters of the terminal device based on the Euler angle perturbation model;

[0013] The Euler angle perturbation model can be either the forward accumulation method or the backward synthesis method of Euler angles;

[0014] The steps for a single iteration based on the Euler angle forward accumulation method are as follows:

[0015] (2) Obtain the Jacobian matrix of the constraint equations:

[0016] The Jacobian matrix of the constraint equations with respect to Euler angles is obtained by differentiation based on the Euler angle forward accumulation method:

[0017]

[0018] Where, θ=[θ x ,θ y ,θ z ] T θ x Let θ be the pitch angle, which is an Euler angle. y Let θ be the yaw angle, which is an Euler angle. z The roll angle is the Euler angle;

[0019] Let P′=R(θ)P+t;

[0020]

[0021] in, ∧ For antisymmetric transformation, i.e., P ∧ Let P be the antisymmetric matrix;

[0022] The Jacobian matrix of the constraint equations is obtained as follows:

[0023]

[0024] Among them, I 3×3 It is a 3D identity matrix; (R(θ)P+t) ∧ It is an antisymmetric matrix of (R(θ)P+t);

[0025] (2) Input the pose parameters (θ,t) of the terminal device in the previous step, and use the Gauss-Newton method to obtain the single-step iteration change based on the Jacobian matrix of the overall error equation.

[0026] [Δθ, Δt] = -(J T J) -1 J T r(θ, t);

[0027] Where [Δθ,Δt] represents the single-step update value of the parameters to be solved by the terminal device;

[0028] (3) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters:

[0029] T(θ,t)←T(Δθ,Δt)T(θ,t);

[0030] (4) Determine the overall error equation r of the information data and the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r is less than the first threshold or the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt] are less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device; if the overall error equation r of the information data is greater than or equal to the first threshold and the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt] are greater than or equal to the second threshold, return to perform the next single-step iteration.

[0031] The single-step iterative steps for the Euler angle-based inverse synthesis method are as follows:

[0032] (2) Obtain the Jacobian matrix of the constraint equations:

[0033] The Jacobian matrix of the constraint equations for Euler angles is obtained by differentiation based on the Euler angle inverse synthesis method:

[0034] The Jacobian matrix of the constraint equations for Euler angles is obtained based on the perturbation model:

[0035]

[0036] The Jacobian matrix of the constraint equations is obtained as follows:

[0037]

[0038] Among them, Q ^ Let Q be the antisymmetric matrix;

[0039] (2) Input the pose parameter values ​​(θ,t) of the previous iteration, and use the Gauss-Newton method to obtain the single-step iteration change based on the Jacobian matrix of the overall error equation.

[0040] [Δθ, Δt] = -(JT J) -1 J T r(θ, t);

[0041] Where [Δθ,Δt] represents the single-step update value of the parameters to be solved by the terminal device;

[0042] (3) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters:

[0043] T(θ,t)←T(Δθ,Δt)T(θ,t);

[0044] The inverse matrix of the updated terminal device pose localization parameters is obtained through a single-step iteration of reverse synthesis.

[0045] T(θ, t) ← T(θ, t) -1 ;

[0046] (4) Determine the overall error equation r of the information data and the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r is less than the first threshold or the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt] are less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device. If the overall error equation r of the information data is greater than or equal to the first threshold and the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt] are greater than or equal to the second threshold, return to perform the next single-step iteration.

[0047] Optionally, the data points in the environmental information data can be the grayscale values ​​of an image, the pixels of an image, or laser point clouds.

[0048] Optionally, when iteratively updating the pose parameters of the terminal device based on the Euler angle perturbation model, if the pose change [θ,t] to be solved is small, the following formula is used to update the pose parameters: [θ,t]←[θ,t]+[Δθ,Δt].

[0049] Compared with the prior art, the present invention can achieve at least the following beneficial effects:

[0050] (1) The positioning method of the present invention simplifies the rotation matrix transformation when the terminal device performs transformation between different coordinates during the self-positioning process, and can realize direct accumulation operation in the iteration process without additional transformation, thereby improving the positioning speed of the terminal device.

[0051] (2) The positioning method of the present invention is based on the intuitiveness of Euler angles, which makes the positioning method of the present invention more reasonably adjustable to adapt to different terminal devices, such as intelligent robots, drones, autonomous driving and other fields. Attached Figure Description

[0052] The accompanying drawings are for illustrative purposes only and are not intended to limit the scope of the invention.

[0053] Figure 1a This is a comparison chart of the single iteration time of the method of the present invention on the KITTI dataset and the single iteration runtime on the visual dataset;

[0054] Figure 1b This is a comparison chart of the single iteration time of the method of the present invention on the TUM dataset and the single iteration time on the visual dataset;

[0055] Figure 2a This is a comparison chart of the iterative curves for solving the error by positive accumulation on the KITTI dataset using the method of the present invention;

[0056] Figure 2b This is a comparison chart of the iterative curves for error backcomposition and solving using the method of the present invention on the KITTI dataset;

[0057] Figure 3a This is a comparison chart of the iterative curves for solving the TUM dataset using the method of the present invention with forward error accumulation.

[0058] Figure 3b This is a comparison chart of the iterative curves for error backcomposition and solving using the method of the present invention on the TUM dataset;

[0059] Figure 4a This is a diagram showing the tracking effect of the method of the present invention on the first frame of the TUM dataset;

[0060] Figure 4b This is a diagram showing the image tracking effect of the method of the present invention after camera movement in the TUM dataset;

[0061] Figure 5a This is a diagram showing the tracking effect of the method of the present invention on the first frame of the KITTI dataset;

[0062] Figure 5b The image shows the effect of the method of the present invention in image tracking after camera movement on the KITTI dataset;

[0063] Figure 6a The original point cloud and the generated point cloud effect diagram are shown for existing technology methods;

[0064] Figure 6b The original point cloud and the generated point cloud effect diagram are shown in the method of the present invention.

[0065] Figure 7 This is a comparison of the single-iteration runtime of the point cloud dataset for the method of the present invention. Detailed Implementation

[0066] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that the method proposed in this invention is not limited to a single sensor and can be used in multi-sensor fusion positioning schemes; therefore, the scope of protection of this invention is not limited to the specific embodiments disclosed below.

[0067] A specific embodiment of the present invention, as shown in Figures 1-7, discloses an agent localization method based on an Euler angle perturbation model. In this example, the rotation adopts the YXZ rotation order in the Earth coordinate system, and the coordinate system adopts a West-Sky-North coordinate system. The Euler angle-based algorithm proposed in this invention simplifies the differentiation process and reduces the computational cost of calculating the Jacobian matrix.

[0068] The specific steps are as follows:

[0069] Step 1: The terminal device acquires environmental information data through the pose sensor and performs preprocessing to filter out unreliable information.

[0070] Optionally, the pose sensor is a camera, LiDAR, or inertial sensor; the pose sensor is mounted on the mobile device.

[0071] Step 2: Establish constraint equations based on the obtained information data.

[0072] The constraint equations established using the obtained information data, such as the reprojection error equations established using visual grayscale, visual pixel coordinates, or laser point cloud coordinates, all contain core constraint equations. The core constraint equations refer to the 3D-to-3D projection error equations after removing the outermost constraints such as photometric consistency or pixel coordinates. This example uses the core constraint equations as an example to illustrate the solution process. The core constraint method is as follows:

[0073] Source feature points of terminal device information data After rotation R∈SO(3) and translation In the new coordinate system, the corresponding coordinates of the source feature points are: The constraint equations are obtained as follows:

[0074] e(θ,t)=||r(θ,t)||2=||T(θ,t)PQ||2=||R(θ)P+tQ||2;

[0075] Where [θ,t] represents the pose parameters to be solved for the terminal device, θ represents the Euler angles to be solved, and t represents the translation amount to be solved; r(θ,t)=R(θ)P+tQ is the error equation, R(θ) represents the rotation matrix corresponding to the Euler angle θ; T(θ,t) is the 4D transformation matrix;

[0076]

[0077] Step 3: Iteratively update the pose parameters of the terminal device based on the Euler angle perturbation model;

[0078] The Euler angle perturbation model can be either the forward accumulation method or the backward synthesis method of Euler angles;

[0079] The steps for a single iteration based on the Euler angle forward accumulation method are as follows:

[0080] (1) Obtain the Jacobian matrix of the constraint equations:

[0081] The present invention proposes a method for obtaining the Jacobian matrix of the constraint equations with respect to Euler angles by taking the derivative of the Euler angle perturbation model:

[0082]

[0083] Where, θ=[θ x ,θ y ,θ z ] T θ x Let θ be the pitch angle, which is an Euler angle. y Let θ be the yaw angle. z This refers to the roll angle;

[0084] Let P′=R(θ)P+t;

[0085]

[0086] in, ∧ For antisymmetric transformation, i.e., P ∧ The antisymmetric matrix of P is shown below:

[0087]

[0088] Where, p x p x p z Let P be the three-dimensional coordinates.

[0089] The Jacobian matrix of the constraint equations is obtained as follows:

[0090]

[0091] Where θ is the Euler angle to be solved for the terminal device; t is the 3D motion offset to be solved for the terminal device; R(θ) is the rotation matrix corresponding to the Euler angle θ to be solved for the terminal device; P is the 3D point coordinates of the data points in the environmental information; I 3×3 It is a 3D identity matrix; (R(θ)P+t) ∧ It is an antisymmetric matrix of (R(θ)P+t);

[0092] (2) Input the pose parameter values ​​(θ,t) of the previous iteration, and use the Gauss-Newton method to obtain the single-step iteration change based on the Jacobian matrix of the overall error equation.

[0093] [Δθ, Δt] = -(J T J) -1 J T r(θ, t);

[0094] Optionally, (θ,t) can be initialized to (0,0).

[0095] Where r is the overall error equation of the information data; [Δθ,Δt] is the single-step update value of the parameter to be solved by the terminal device;

[0096] (2) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters:

[0097] T(θ,t)←T(Δθ,Δt)T(θ,t);

[0098] When the pose change [θ,t] to be solved is known to be small, the pose parameters are updated using the following formula:

[0099] [θ,t]←[θ,t]+[Δθ,Δt].

[0100] (4) Determine the overall error equation r of the information data and the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r is less than the first threshold or the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt] are less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device. If the overall error equation r of the information data is greater than or equal to the first threshold and the updated values ​​of the parameters to be solved by the terminal device [Δθ, Δt] are greater than or equal to the second threshold, return to perform the next single-step iteration.

[0101] The Jacobian matrix of the error equation obtained using this scheme can greatly simplify the calculation of the Jacobian, thereby improving the efficiency of positioning.

[0102] The single-step iterative steps for the Euler angle-based inverse synthesis method are as follows:

[0103] (3) Obtain the Jacobian matrix of the constraint equations:

[0104] The Jacobian matrix of the constraint equations for Euler angles is obtained using the inverse synthesis method based on Euler angles:

[0105]

[0106] The Jacobian matrix of the constraint equations is obtained as follows:

[0107]

[0108] Among them, Q ∧ Let Q be the antisymmetric matrix;

[0109] (2) Input the pose parameter values ​​(θ, t) of the previous iteration, and use the Gauss-Newton method to obtain the single-step iteration change based on the Jacobian matrix of the overall error equation.

[0110] [Δθ, Δt] = -(J T J) -1 J T r(θ, t);

[0111] Optionally, (θ, t) can be initialized to (0, 0).

[0112] Where r is the overall error equation of the information data; [Δθ, Δt] are the single-step update values ​​of the parameters to be solved by the terminal device;

[0113] (3) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters:

[0114] T(θ,t)←T(Δθ,Δt)T(θ,t);

[0115] When the pose change [θ, t] to be solved is known to be small, the pose parameters are updated using the following formula:

[0116] [θ,t]←[θ,t]+[Δθ,Δt]. The transformation matrix T formed by [θ,t] obtained iteratively using the inverse synthesis algorithm is the reciprocal of the transformation matrix to be solved. The inverse synthesis update of the single-step iteration yields the reciprocal matrix of the updated terminal device pose localization parameters, thus obtaining the updated terminal device pose localization parameters obtained in the single-step iteration.

[0117] T(θ, t) ← T(θ, t) -1 .

[0118] (4) Determine the overall error equation r of the information data and the updated value of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r is less than the first threshold or the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device; if the overall error equation r of the information data is greater than or equal to the first threshold and the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is greater than or equal to the second threshold, return to perform the next single-step iteration.

[0119] To illustrate the effectiveness of the method proposed in this invention, the above technical solution of this invention will be described in detail below through specific embodiments.

[0120] Example 1

[0121] This embodiment is based on the terminal device positioning method of the present invention, which obtains the terminal device positioning by acquiring image information through a camera installed on the terminal device. The specific steps are as follows:

[0122] Step 1: The camera of the terminal device acquires two consecutive frames, I1 and I2; the image feature points p corresponding to these two frames, I1 and I2, are obtained. i and q i ,(i=1,2,…,N),p i Let q be the i-th image feature point in image I1. i P is the i-th image feature point in image I2; i =[p ix ,p iy ,p iz ] T and Q i =[q ix ,q iy ,q iz ] T Image feature points p i and q i 3D coordinates;

[0123] Step 2: Using image I1 as the reference frame, the visual odometry of the terminal device establishes the error constraint equation using the direct method:

[0124]

[0125] Where, r i (.) represents the error equation for the i-th image feature point; I1(p i ) and I2(q i ) corresponds to the image feature point p i and q i The grayscale values ​​in the two frames; π(.) represents the grayscale value of point P. i Mapped to a two-dimensional image coordinate system:

[0126]

[0127] Among them, f x f y For the vertical and horizontal focal lengths of the camera, respectively; [c x c y ] represents the center pixel coordinates of the 2D image; T(θ,t,P) i ) indicates that point p i 3D coordinates P i Transform to the new coordinate system, i.e.: T(θ, t, P) i )=T(θ,t)P i .

[0128] Step 3: Determine the location of the terminal device using visual odometry based on the Euler angle forward accumulation method or the reverse synthesis method;

[0129] For the forward accumulation method based on Euler angles:

[0130] (1) The iterative formula for visual photometric error using the positive accumulation method based on Euler angles:

[0131]

[0132] Where [Δθ,Δt] are the updated values ​​of the parameters to be solved for the terminal device;

[0133] (2) Obtain the change in a single iteration:

[0134] Obtain the error equation r for each image feature point i (θ,t) Jacobian matrix:

[0135]

[0136] in, For image feature points q i Image gradient;

[0137]

[0138]

[0139] Where, (T(θ,t,P) i )) ∧ Let T(θ, t, P) i The antisymmetric matrix of )

[0140] The overall error equation Jacobian matrix is ​​obtained based on the Jacobian matrix of each image feature point:

[0141]

[0142] Input the pose parameter values ​​(θ,t) from the previous iteration, and obtain the single-step iteration change based on the Jacobian matrix of the overall error equation.

[0143] [Δθ,Δt]=-(J T J) -1 J T r;

[0144] Where r is the global error equation for image feature points.

[0145] Optionally, (θ,t) can be initialized to (0,0).

[0146] (3) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters:

[0147] [θ,t]←[θ,t]+[Δθ,Δt].

[0148] Because the terminal device pose parameters are updated using a direct visual method, the parallax between two frames is small.

[0149] (4) Determine the overall error equation r of the image feature points and the updated value of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r of the image feature points is less than the first threshold or the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device; if the overall error equation r of the image feature points is greater than or equal to the first threshold and the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is greater than or equal to the second threshold, return to perform the next single-step iteration.

[0150] For the inverse synthesis method based on Euler angles:

[0151] (1) Iterative formula for visual photometric error using Euler angle-based inverse synthesis:

[0152]

[0153] Where [Δθ,Δt] are the updated values ​​of the parameters to be solved for the terminal device;

[0154] (2) Obtain the change in a single iteration:

[0155] Obtain the Jacobian matrix, the error equation for each image feature point:

[0156]

[0157] in, For image feature point p i The gradient.

[0158]

[0159]

[0160] in, For P i The antisymmetric matrix;

[0161] As can be seen from the above, the Jacobian of the inverse synthesis method can be calculated in advance and does not need to be recalculated in the iteration.

[0162] The overall error equation Jacobian is obtained based on the Jacobian matrix of each image feature point:

[0163]

[0164] Input the pose parameter values ​​(θ,t) from the previous iteration, and obtain the single-step iteration change based on the Jacobian matrix of the overall error equation.

[0165] [Δθ,Δt]=-(J T J) -1 J T r(θ,t);

[0166] Where r is the global error equation for image feature points.

[0167] Optionally, (θ,t) can be initialized to (0,0).

[0168] (3) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters:

[0169] [θ,t]←[θ,t]+[Δθ,Δt].

[0170] (4) Determine the overall error equation r of the image feature points and the updated value of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r of the image feature points is less than the first threshold or the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device; if the overall error equation r of the image feature points is greater than or equal to the first threshold and the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is greater than or equal to the second threshold, return to perform the next single-step iteration.

[0171] The transformation matrix T, formed by [θ,t] obtained iteratively using the inverse synthesis algorithm, is the reciprocal of the transformation matrix to be solved.

[0172] T(θ,t)←T(θ,t) -1 .

[0173] Example test of Example 1

[0174] This example uses two main open-source visual datasets: the KITTI dataset and the TUM dataset. Both datasets contain depth maps of consecutive frame images and the initial frame image. The test computer used in this example is a NUCi7KYK.

[0175] The aforementioned forward accumulation (SE3FA) and backward synthesis (SE3IC) algorithms based on Lie algebras, as well as the traditional solution algorithm based on Euler angles (Euler_com_J), and the proposed forward accumulation (EulerFA) and backward synthesis (EulerIC) algorithm based on Euler angles were tested on the open-source datasets KITTI and TUM. The time consumption per iteration is shown in the box plot in Figure 1. In Figure 1, the two methods proposed in this invention are indicated by *, and the average time consumption per iteration for each algorithm is indicated by △. As can be seen from Figure 1, the forward accumulation algorithm based on Euler angles (EulerFA) proposed in this invention has a shorter time consumption per iteration compared to the other two forward accumulation solution algorithms (SE3FA and Euler_com_J). The Euler angle-based inverse synthesis algorithm (EulerIC) proposed in this invention reduces the time consumption per iteration compared to the Lie algebra-based inverse synthesis algorithm (SE3IC). Furthermore, compared to the forward accumulation algorithm, the overall time consumption per iteration of the inverse synthesis algorithm is significantly reduced.

[0176] Furthermore, Figures 2 and 3 show the error reduction of each algorithm on the KITTI and TUM datasets, respectively. As can be seen from the figures, the algorithm proposed in this invention can obtain a solution range similar to existing algorithms, demonstrating the effectiveness of the proposed algorithm. In addition, the experimental results in Figure 2 also show that the forward accumulation algorithm requires fewer iterations, but its single iteration time is relatively long. Both the forward accumulation algorithm and the reverse synthesis algorithm have their respective application scenarios.

[0177] Figures 4 and 5 show the pixel tracking results converted from the pose calculations obtained by each algorithm. In the test, all five algorithms described in this invention converged, yielding consistent pixel tracking results, demonstrating the reliability of the solutions provided by the algorithm of this invention. This concludes the demonstration that the algorithm of this invention can converge, obtain correct solutions, and further reduce computational load, accelerating the operation. Furthermore, the Euler angles used in this invention also facilitate debugging.

[0178] Example 2

[0179] This embodiment is based on the terminal device positioning method of the present invention, which obtains the terminal device positioning by collecting point cloud information through a laser sensor installed on the terminal device. The specific steps are as follows:

[0180] Step 1: The laser sensor of the terminal device acquires the source point cloud cluster C1. The laser sensor rotates the source point cloud cluster C1 by R∈SO(3) and translates it. Subsequently, in the target laser coordinate system, the source point cloud cluster C1 is transformed into the target point cloud cluster C1, P i and Q iThese represent the positions of the same point in the source point cloud cluster C1 and the target point cloud cluster C2, respectively: P i =[p ix ,p iy ,p iz ] T ∈C1 and Q i =[q ix ,q iy ,q iz ] T ∈C2(i=1,…,).

[0181] Step 2: Establish constraint equations using source point cloud cluster C1 and target point cloud cluster C2:

[0182]

[0183] Step 3: Determine the location of the terminal device using the laser odometry based on the Euler angle forward accumulation method or the reverse synthesis method;

[0184] For the Euler angle-based forward accumulation method:

[0185] (1) The iterative formula for visual photometric error based on the Euler angle positive accumulation method is as follows:

[0186]

[0187] (2) Obtain the error equation r for each point i Jacobian matrix of (θ,t):

[0188]

[0189] Based on the error equation r for each point in point cloud cluster C1 i The Jacobian matrix of (θ,t) yields the Jacobian of the global error equation:

[0190]

[0191] Input the pose parameter values ​​(θ,t) from the previous iteration, and obtain the single-step iteration change based on the Jacobian matrix of the overall error equation:

[0192] [Δθ,Δt]=-(J T J) -1 J T r(θ,t);

[0193] Where r is the global error equation for image feature points.

[0194] Optionally, (θ,t) can be initialized to (0,0).

[0195] (3) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters:

[0196] T(θ,t)←T(Δθ,Δt)T(θ,t).

[0197] When the pose change is small, the updated terminal device pose positioning parameters are obtained by single-step iteration using the following method:

[0198] [θ,t]←[θ,t]+[Δθ,Δt].

[0199] (4) Determine the overall error equation r of the image feature points and the updated value of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r of the image feature points is less than the first threshold or the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device; if the overall error equation r of the image feature points is greater than or equal to the first threshold and the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is greater than or equal to the second threshold, return to perform the next single-step iteration.

[0200] For the inverse synthesis method based on Euler angles:

[0201] (1) Iterative formula for visual photometric error using Euler angle-based inverse synthesis:

[0202]

[0203] (2) Obtain the error equation r for each point i Jacobian matrix of (θ,t):

[0204]

[0205] The overall error equation Jacobian matrix is ​​obtained based on the Jacobian matrix of the error equation for each point in the target point cloud cluster C2:

[0206]

[0207] Input the pose parameter values ​​(θ,t) from the previous iteration, and obtain the single-step iteration change based on the Jacobian matrix of the overall error equation:

[0208] [Δθ,Δt]=-(J T J) -1 J T r(θ,t);

[0209] Where r is the global error equation for image feature points. Optionally, (θ,t) can be initialized to (0,0).

[0210] (3) Based on the obtained single-step iteration change, the updated terminal device pose positioning parameters are obtained:

[0211] T(θ,t)←T(Δθ,Δt)T(θ,t).

[0212] When the pose change is small, the following formula is used to update the parameters:

[0213] [θ,t]←[θ,t]+[Δθ,Δt].

[0214] The transformation matrix T formed by [θ,t] obtained iteratively using the inverse synthesis algorithm is the reciprocal of the transformation matrix to be solved:

[0215] T(θ,t)←T(θ,t) -1 .

[0216] (4) Determine the overall error equation r of the image feature points and the updated value of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r of the image feature points is less than the first threshold or the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device; if the overall error equation r of the image feature points is greater than or equal to the first threshold and the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is greater than or equal to the second threshold, return to perform the next single-step iteration.

[0217] The average time consumed per iteration for each algorithm is shown in Table 1. Table 1 compares the average time consumed per iteration of the existing forward accumulation algorithms (SE3FA and EulerFA_com_J) with the forward accumulation algorithm (EulerFA) based on the Euler angle perturbation model proposed in this invention, and compares the average time consumed per iteration of the existing backward synthesis algorithm (SE3IC) with the backward synthesis algorithm (EulerIC) based on Euler angles proposed in this invention.

[0218] Example 2 Case Study

[0219] This example uses Stanford University's open-source point cloud dataset for testing. The point cloud dataset consists of a set of 3D points. The original point cloud and the point cloud after rotation and offset with added noise are shown in the figure. The computer model used to run this example is NUCi7KYK.

[0220] This example uses three commonly used methods: the forward accumulation algorithm based on Lie algebras (SE3FA), the backward synthesis algorithm based on Lie algebras (SE3IC), and the traditional solution algorithm based on Euler angles (EulerFA_com_J). It also uses the forward solution algorithm (EulerFA) and the backward synthesis algorithm (EulerIC) based on the Euler angle perturbation model proposed in this invention to solve the coordinate system transformation relationship between two point cloud clusters and achieve point cloud registration, thereby comparing the performance of each algorithm. Experimental results show that each algorithm can obtain approximate pose values. The calculation results are accumulated and added to the generated point cloud, and the resulting point cloud registration result is shown in the figure. The figure shows that the original point cloud and the generated point cloud basically overlap, indicating the reliability of the solution result.

[0221] Furthermore, the box plot of the time consumed in a single iteration of point cloud data registration is shown in the figure. Similarly, * indicates the two methods proposed in this invention, and △ indicates the average time consumed in a single iteration for each algorithm. As can be seen from the figure, the Euler angle-based forward accumulation algorithm (EulerFA) proposed in this invention still has a shorter single iteration time for point cloud processing compared to the other two forward accumulation algorithms (SE3FA and Euler_com_J). The Euler angle-based backward synthesis algorithm (EulerIC) proposed in this invention also shows a reduction in single iteration time compared to the Lie algebra-based backward synthesis algorithm (SE3IC).

[0222] (3) Comprehensive data

[0223] Table 1 lists the average running time of a single iteration for several algorithms for solving pose using visual image datasets and laser point cloud datasets.

[0224] Table 1 Comparison of the average iteration time of each algorithm

[0225]

[0226] The percentage decrease is calculated as: (XX - EulerFA / IC) / XX * 100%.

[0227] As shown in Table 1, for the KITTI dataset, the proposed Euler angle perturbation-based forward accumulation algorithm (EulerFA) reduces the time consumption per iteration by 0.9% and 5.3% respectively compared to the two existing forward accumulation algorithms. The proposed Euler angle-based inverse synthesis algorithm (EulerIC) reduces the time consumption per iteration by 8.2% compared to the Lie algebra-based inverse synthesis algorithm (SE3IC), indicating that the algorithm of this invention can further reduce the computational load while ensuring convergence in practical applications. Similarly, the method proposed in this invention shortens the solution time on the TUM visual dataset and the laser point cloud dataset while ensuring solution accuracy.

[0228] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for agent localization based on an Euler angle perturbation model, characterized in that, The specific steps are as follows: Step 1: The terminal device acquires its own and environmental information data through the posture sensor; Step 2: Establish constraint equations based on the obtained information data: Source feature points of information data acquired by terminal devices After rotation R∈SO(3) and translation In the new coordinate system, the corresponding coordinates of the source feature points are: The constraint equations are obtained as follows: e(θ,t)=‖r(θ,t)‖2=‖T(θ,t)PQ‖2=‖R(θ)P+tQ‖2; Where (θ,t) are the pose parameters to be solved for the terminal device, θ are the Euler angles to be solved for the terminal device, and t are the translation amounts to be solved for the terminal device; r(θ,t)=R(θ)P+tQ is the global error equation, R(θ) represents the rotation matrix corresponding to the Euler angle θ; T(θ,t) is the 4-dimensional transformation matrix; Step 3: Iteratively update the pose parameters of the terminal device based on the Euler angle perturbation model; The Euler angle perturbation model can be either the forward accumulation method or the backward synthesis method of Euler angles; When iteratively updating the pose parameters of a terminal device based on the Euler angle perturbation model, if the pose change [θ,t] to be solved is small, the following formula is used to update the pose parameters: [θ,t]←[θ,t]+[Δθ,Δt]; The steps for a single iteration based on the Euler angle forward accumulation method are as follows: (1) Obtain the Jacobian matrix of the constraint equations: The Jacobian matrix of the constraint equations with respect to Euler angles is obtained by differentiation based on the Euler angle forward accumulation method: Where, θ=[θ x ,θ y ,θ z ] T θ x Let θ be the pitch angle, which is an Euler angle. y Let θ be the yaw angle, which is an Euler angle. z The roll angle is the Euler angle; Let P′=R(θ)P+t; Where ∧ represents the antisymmetric transformation, i.e., P ∧ Let P be the antisymmetric matrix; The Jacobian matrix of the constraint equations is obtained as follows: Among them, I 3×3 It is a 3D identity matrix; (R(θ)P+t) ∧ It is an antisymmetric matrix of (R(θ)P+t); (2) Input the pose parameters (θ,t) of the terminal device in the previous step, and use the Gauss-Newton method to obtain the single-step iteration change based on the Jacobian matrix of the overall error equation. [Δθ,Δt]=-(J T J) -1 J T r(θ,t); Where [Δθ,Δt] represents the single-step update value of the parameters to be solved by the terminal device; (3) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters: T(θ,t)←T(Δθ,Δt)T(θ,t); (4) Determine the overall error equation r of the information data and the updated value of the terminal device's unsolved parameter [Δθ, Δt]. If the overall error equation r is less than the first threshold or the updated value of the terminal device's unsolved parameter [Δθ, Δt] is less than the second threshold, exit the iteration and obtain the final value of the terminal device's pose positioning parameter; if the overall error equation r of the information data is greater than or equal to the first threshold and the updated value of the terminal device's unsolved parameter [Δθ, Δt] is greater than or equal to the second threshold, return to perform the next single-step iteration. The single-step iterative steps for the Euler angle-based inverse synthesis method are as follows: (1) Obtain the Jacobian matrix of the constraint equations: The Jacobian matrix of the constraint equations for Euler angles is obtained by differentiation based on the Euler angle inverse synthesis method: The Jacobian matrix of the constraint equations for Euler angles is obtained based on the perturbation model: The Jacobian matrix of the constraint equations is obtained as follows: Among them, Q ∧ Let Q be the antisymmetric matrix; (2) Input the pose parameter values ​​(θ,t) of the previous iteration, and use the Gauss-Newton method to obtain the single-step iteration change based on the Jacobian matrix of the overall error equation. [Δθ,Δt]=-(J T J) -1 J T r(θ,t); Where [Δθ,Δt] represents the single-step update value of the parameters to be solved by the terminal device; (3) Based on the single-step iteration change, perform single-step iteration to obtain updated terminal device pose positioning parameters: T(θ,t)←T(Δθ,Δt)T(θ,t); The inverse matrix of the updated terminal device pose localization parameters is obtained through a single-step iteration of reverse synthesis. T(θ,t)←T(θ,t) -1 ; (4) Determine the overall error equation r of the information data and the updated value of the parameters to be solved by the terminal device [Δθ, Δt]. If the overall error equation r is less than the first threshold or the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is less than the second threshold, exit the iteration and obtain the final value of the pose positioning parameters of the terminal device; if the overall error equation r of the information data is greater than or equal to the first threshold and the updated value of the parameters to be solved by the terminal device [Δθ, Δt] is greater than or equal to the second threshold, return to perform the next single-step iteration.

2. The intelligent agent localization method according to claim 1, characterized in that, The data points in the environmental information data are the grayscale values ​​of the image, the pixels of the image, or the laser point cloud.

Images