CN-116337064-B - Terminal equipment positioning method based on Euler angle disturbance model

CN116337064BCN 116337064 BCN116337064 BCN 116337064BCN-116337064-B

Abstract

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

Inventors

CAI ZHIHAO
NIE BAOZHEN
WANG YINGXUN
ZHAO JIANG

Assignees

北京航空航天大学

Dates

Publication Date: 20260512
Application Date: 20221202

Claims (2)

1. The intelligent body positioning method based on the Euler angle disturbance model is characterized by comprising the following specific steps of: step 1, terminal equipment acquires self and environmental information data through a pose sensor; Step 2, establishing a constraint equation based on the obtained information data: Source characteristic point of information data acquired by terminal equipment Through rotation R epsilon SO (3) and translation In the new coordinate system, the corresponding coordinates of the source characteristic points in the new coordinate system are as follows Obtaining a constraint equation: e(θ,t)=‖r(θ,t)‖ 2 ＝‖T(θ,t)P-Q‖ 2 ＝‖R(θ)P+t-Q‖ 2 ; wherein (theta, T) is a pose parameter to be solved of the terminal equipment, theta is an Euler angle to be solved of the terminal equipment, T is a translation amount to be solved of the terminal equipment, R (theta, T) =R (theta) P+t-Q is an integral error equation, R (theta) represents a rotation matrix corresponding to the Euler angle theta, and T (theta, T) is a 4-dimensional transformation matrix; step 3, carrying out iterative updating on pose parameters of the terminal equipment based on the Euler angle disturbance model; The Euler angle disturbance model is Euler angle forward addition method or reverse synthesis method; When the pose parameters of the terminal equipment are iteratively updated based on the Euler angle disturbance model, when the pose change [ theta, t ] to be solved is known to be smaller, the pose parameters are updated by using the following formula [ theta, t ] ≡ [ theta, t ] + [ delta theta, delta t ]; the single iteration step for the euler angle forward addition method is as follows: (1) Obtaining a jacobian matrix of the constraint equation: obtaining a jacobian matrix of the constraint equation for the Euler angle based on the derivative of the Euler angle forward addition method: Wherein θ= [ θ x ,θ y ,θ z ] T ,θ x ] is the pitch angle of the euler angle, θ y is the yaw angle of the euler angle, and θ z is the roll angle of the euler angle; Let P' =r (θ) p+t; wherein, Λ is an antisymmetric transformation, namely P ∧ is an antisymmetric matrix of P; Obtaining a jacobian matrix of a constraint equation: wherein, I 3×3 is a 3-dimensional identity matrix, R (theta) P+t ∧ is an antisymmetric matrix of (R (theta) P+t); (2) Inputting pose parameters (theta, t) of previous terminal equipment, and obtaining single-step iteration variable quantity by using a Gauss Newton method based on an overall error equation jacobian matrix; [Δθ,Δt]=-(J T J) -1 J T r(θ,t); wherein [ delta theta, delta t ] is a single-step updated value of parameters to be solved of the terminal equipment; (3) Based on the single-step iteration variable quantity, the updated terminal equipment pose positioning parameters are obtained through single-step iteration: T(θ,t)←T(Δθ,Δt)T(θ,t); (4) Judging the integral error equation r of the information data and the parameter updating value [ delta theta, delta t ] to be solved of the terminal equipment, and when the integral error equation r is smaller than a first threshold value or the parameter updating value [ delta theta, delta t ] to be solved of the terminal equipment is smaller than a second threshold value, exiting from iteration to obtain the final value of the pose positioning parameter of the terminal equipment; the single-step iterative procedure for the euler angle-based reverse synthesis is as follows: (1) Obtaining a jacobian matrix of a constraint equation: Obtaining a jacobian matrix of the constraint equation for the Euler angles based on derivative of the Euler angle reverse synthesis method: And (3) solving a jacobian matrix of a constraint equation for the Euler angle based on the disturbance model: Obtaining a jacobian matrix of a constraint equation: wherein Q ∧ is an antisymmetric matrix of Q; (2) Inputting pose parameter values (theta, t) of the previous iteration, and obtaining single-step iteration variable quantity by using a Gauss Newton method based on an overall error equation jacobian matrix; [Δθ,Δt]=-(J T J) -1 J T r(θ,t); wherein [ delta theta, delta t ] is a single-step updated value of parameters to be solved of the terminal equipment; (3) Based on the single-step iteration variable quantity, the updated terminal equipment pose positioning parameters are obtained through single-step iteration: T(θ,t)←T(Δθ,Δt)T(θ,t); Reversely synthesizing the updated single-step iteration to obtain an inverse matrix of the updated terminal equipment pose positioning parameters, and obtaining the updated terminal equipment pose positioning parameters through the single-step iteration: T(θ,t)←T(θ,t) -1 ; (4) Judging the integral error equation r of the information data and the parameter updating value [ delta theta, delta t ] to be solved of the terminal equipment, when the integral error equation r is smaller than a first threshold value or the parameter updating value [ delta theta, delta t ] to be solved of the terminal equipment is smaller than a second threshold value, exiting from iteration to obtain the final value of the pose positioning parameter of the terminal equipment, and if the integral error equation r of the information data is larger than or equal to the first threshold value and the parameter updating value [ delta theta, delta t ] to be solved of the terminal equipment is larger than or equal to the second threshold value, returning to the next single step iteration.
2. The agent localization method of claim 1, wherein the data points in the environmental information data are gray values of an image, pixels of an image, or a laser point cloud.

Description

Terminal equipment positioning method based on Euler angle disturbance model Technical Field The invention belongs to the technical field of automatic positioning, and relates to a terminal equipment positioning method based on an Euler angle disturbance model. Background With the wide application and development of various terminal devices (such as robots, unmanned aerial vehicles, etc.), autonomous positioning technology of the terminal devices is becoming more and more indispensable. The autonomous positioning comprises position analysis and gesture analysis of the terminal equipment, and the gesture information of the terminal equipment is obtained through analysis of information measured by a sensor carried by the terminal equipment. The gesture analysis is mostly obtained by an inertial sensor, but in a complex and changeable real environment, a single sensor which is used for acquiring gesture information by means of a gyroscope and the like is not robust enough, so that the conventional method is added with passive sensor information such as vision, laser point cloud and the like to further restrict the accumulated error problem caused by a single odometer. The pose solving thought commonly used at present is to utilize information obtained by various sensors to establish constraint, and obtain pose information by a least square descent iteration method. However, due to redundancy of rotation matrix parameters and cumulative characteristics thereof, the conventional iterative descent method cannot directly solve the rotation motion through the rotation matrix. The method effectively solves the problem of completing rotary motion solving through descending iteration, but the lie algebra needs e index and antisymmetric operation to complete parameter updating iteration, and has insufficient intuitiveness. In addition to using lie algebra to accomplish the solution of the pose, some positioning methods based on euler angles to solve the pose are proposed, but such methods generally need to be based on more complex jacobian matrices, and therefore have low efficiency relative to the lie algebra method. Disclosure of Invention In view of the above problems, the invention provides a terminal equipment positioning method based on an Euler angle disturbance model, which can improve the acquisition efficiency and accuracy of acquiring the pose of the terminal equipment. The invention provides an agent positioning method based on an Euler angle disturbance model, which comprises the following specific steps: step 1, terminal equipment acquires self and environmental information data through a pose sensor; Step 2, establishing a constraint equation based on the obtained information data: Source characteristic point of information data acquired by terminal equipment Through rotation R epsilon SO (3) and translationIn the new coordinate system, the corresponding coordinates of the source characteristic points in the new coordinate system are as followsObtaining a constraint equation: e(θ,t)=||r(θ,t)||2＝||T(θ,t)P-Q||2＝||R(θ)P+t-Q||2; wherein (theta, T) is a pose parameter to be solved of the terminal equipment, theta is an Euler angle to be solved of the terminal equipment, T is a translation amount to be solved of the terminal equipment, R (theta, T) =R (theta) P+t-Q is an integral error equation, R (theta) represents a rotation matrix corresponding to the Euler angle theta, and T (theta, T) is a 4-dimensional transformation matrix; step 3, carrying out iterative updating on pose parameters of the terminal equipment based on the Euler angle disturbance model; The Euler angle disturbance model is Euler angle forward addition method or reverse synthesis method; the single iteration step for the euler angle forward addition method is as follows: (2) Obtaining a jacobian matrix of the constraint equation: obtaining a jacobian matrix of the constraint equation for the Euler angle based on the derivative of the Euler angle forward addition method: Wherein θ= [ θ x,θy,θz]T,θx ] is the pitch angle of the euler angle, θ y is the yaw angle of the euler angle, and θ z is the roll angle of the euler angle; Let P' =r (θ) p+t; Wherein ∧ is an antisymmetric transformation, i.e., P ∧ is an antisymmetric matrix of P; Obtaining a jacobian matrix of a constraint equation: wherein, I 3×3 is a 3-dimensional identity matrix, R (theta) P+t ∧ is an antisymmetric matrix of (R (theta) P+t); (2) Inputting pose parameters (theta, t) of previous terminal equipment, and obtaining single-step iteration variable quantity by using a Gauss Newton method based on an overall error equation jacobian matrix; [Δθ,Δt]=-(JTJ)-1JTr(θ,t); wherein [ delta theta, delta t ] is a single-step updated value of parameters to be solved of the terminal equipment; (3) Based on the single-step iteration variable quantity, the updated terminal equipment pose positioning parameters are obtained through single-step iteration: T(θ,t)←T(Δθ,Δt)T(θ,t); (4) Judging the int