A novel design method for cooperative target pose observer based on potential functions

Abstract

This invention discloses a design method for a cooperative target pose observer based on a novel potential function, used for real-time position and attitude estimation of cooperative targets in space. Common pose estimation methods typically include static and dynamic methods. Dynamic methods combine observation information with the system's kinematic or dynamic information, and under the same conditions, usually yield better estimation results than static methods. As a dynamic estimation method, the observer is valued for its simple structure and ease of application. Addressing the problem of the limited variety of potential functions commonly used in current pose estimation observers, this invention, based on existing potential functions, proposes a method for constructing an observer based on a novel potential function. This method can be used for synchronous relative pose estimation during operations by a space robotic arm on a cooperative target. The observer derived from the novel potential function designed in this invention not only possesses the same theoretical stability characteristics as existing observers but also demonstrates better estimation performance in practical applications.

Description

Technical Field

[0001] This invention belongs to the field of navigation, guidance and control, and particularly relates to a design method for a cooperative target pose observer based on a novel potential function. Background Technology

[0002] In recent years, with the development of unmanned aerial vehicles (UAVs) and autonomous underwater vehicles (AUVs), pose (position and attitude) estimation algorithms have become an increasingly important research subject. These algorithms can be used not only for autonomous navigation of aircraft but also for relative navigation of collaborative objects operated by space robotic arms. Common pose estimation methods typically include static and dynamic methods. The former utilizes only the system's observation information, while the latter combines observation information with the system's kinematic or dynamic information, generally yielding better estimation results under the same conditions. Currently, commonly used dynamic pose estimation methods include filters and observers. The former utilizes the statistical characteristics of noise for design, making it difficult to analyze stability theoretically; common filters include EKF, UKF, and PF. The latter, however, is designed within a deterministic framework and can establish strict stability.

[0003] With the rapid development of nonlinear attitude or pose observers, the use of potential function gradients to design observer correction terms and construct stable observers has gradually attracted attention. Introducing potential function gradient-based correction terms into the system's kinematic or dynamic equations allows for the design of almost globally asymptotically stable pose observers (i.e., the observer converges at any point in the manifold space except when the system is initialized at a finite number of unstable critical points), guaranteeing near-global stability. However, the types of potential functions available for observer design are too limited, and observers designed based on existing potential functions have significant room for improvement in pose estimation performance. Summary of the Invention

[0004] The problem solved by this invention is to overcome the shortcomings of existing solutions and provide a highly feasible design method for a cooperative target pose observer based on a novel potential function. This method can be widely applied to pose estimation problems of spacecraft, unmanned aerial vehicles, and autonomous underwater vehicles, and can also be used for pose estimation of cooperative targets relative to the operating platform in space robotic arm operations.

[0005] The objective of this invention is achieved through the following technical solution: a design method for a cooperative target pose observer based on a novel potential function, the method comprising the following steps:

[0006] Step S1: Design an error term that can characterize the estimated pose information and the true pose information of the cooperative target relative to the operating platform. The error term should have a relatively simple expression and be able to reflect the deviation between the true relative pose information and the estimated pose information provided by the observer in a true and effective manner.

[0007] Step S2: Design a potential function based on the error term provided in Step S1, which can reflect the energy properties of the potential function. The potential function should meet the following requirements: it is a non-negative, continuously differentiable function with respect to the error state; for any error state, the potential function is always non-negative; the potential function is equal to zero if and only if the error term is a unit value, that is, the estimated state is equal to the true state.

[0008] Step S3: Calculate the gradient of the potential function designed in step S2, and based on the nonlinear control theory, further develop a correction term that can be introduced into the system's kinematics or dynamics equations.

[0009] Step S4: Introduce the correction term designed in step S3 into the kinematic or dynamic equations of the system, and combine it with nonlinear control theory to ensure that the designed observer is theoretically stable; if the observer is unstable, return to step S2 and redesign the potential function based on the error function.

[0010] The advantages and beneficial effects of this invention are as follows:

[0011] This invention derives a pose observer based on a potential function with consistent formal complexity through relatively simple mathematical derivation. The designed observer achieves better estimation results than existing observers. Furthermore, by selectively adjusting the observer parameters, the estimation characteristics of the observer during the convergence and stable estimation phases can be adjusted to obtain even more ideal estimation results.

[0012] This invention can develop a series of new potential functions based on existing potential functions, and use the new potential functions to design a series of new pose estimation observers, providing users with a wider range of choices. Attached Figure Description

[0013] Figure 1 This is a schematic diagram of the robotic arm platform and the cooperative target.

[0014] Figure 2 It is the observer attitude estimation error.

[0015] Figure 3 It is the observer position estimation error.

[0016] Figure 4 It is the observer angular velocity bias estimation error.

[0017] Figure 5 It is the observer linear velocity bias estimation error.

[0018] Figure 6 This is a flowchart of the present invention. Detailed Implementation

[0019] The following will describe in more detail the specific implementation process of the cooperative target pose observer design method based on a novel potential function involved in this invention. While this section provides exemplary implementation processes of this disclosure, it should be understood that this disclosure can be implemented in various forms and should not be limited to the implementation processes described herein. That is, the ideas of constructing novel potential functions and observers disclosed herein can be further applied and extended to the design of other types of novel potential functions and observers. Rather, this exemplary implementation process is provided to enable a more thorough understanding of this disclosure and to fully convey the scope of this disclosure to those skilled in the art. It should be noted that, unless otherwise specified, the exemplary implementation processes and features therein in this invention can be combined with each other.

[0020] Before detailing the specific embodiments of the present invention, it is necessary to provide some prior knowledge. In the following description, and They represent the set of real numbers, the set of non-negative real numbers, and the set of natural numbers, respectively. Describe an n-dimensional Euclidean space. Indicates embedded in An n-dimensional sphere. For any matrix (square matrix), the superscripts "-1" and "T" denote the inverse and transpose of the square matrix, respectively. Its Euclidean inner multiplication is defined as 《A,B》=tr(A T B). For any vector Its Euclidean norm is Let matrix Its Frobenius norm is

[0021] Let the inertial frame of reference be... This system is R∈SO(3) represents the position and attitude of the system relative to the inertial reference frame. and This represents the translational velocity and rotational angular velocity of the system relative to the inertial frame.

[0022] For any Define the mapping (·) × : Satisfy x × y = x × y, where × is the vector cross product operator. Define the mapping vex(·): for(·) × The inverse mapping satisfies for any

[0023] and There is vex(ω) × )=ω and (vex(Ω))× =Ω holds true. The relative pose of a rigid body can be determined by a three-dimensional special orthogonal group. This means, that is:

[0024]

[0025] se(3) denotes the Lie algebra of SE(3), given by the following formula:

[0026]

[0027] Define the wedge product mapping (·):

[0028]

[0029] The tangent space of SE(3) can be represented as T g SE(3) = {gX|g∈SE(3),X∈se(3)}. Define the mapping. Satisfy any square matrix have Established. Define mapping. Satisfy any

[0030] have Established. Set up. but:

[0031]

[0032] Set up a square matrix Define the mapping ψ;

[0033]

[0034] set up Then there is Established. Order I n Represents an n-dimensional identity matrix.

[0035] set up For the angle-axis parameterized operator on SO(3), satisfying for have Established.

[0036] For differentiable smooth functions f: Its gradient is expressed as Define the left-invariant Riemann matrix <·,·> on SE(3): satisfy:

[0037]

[0038] Define the adjoint mapping Ad g (·): For any g∈SE(3),

[0039] Ad g (X)=gXg -1 Established. Its place The matrix on is represented as:

[0040]

[0041] For any g1, g2 ∈ SE(3), have and

[0042] Indicates Ad g The transpose of (·) is Then we have:

[0043]

[0044] Let g∈SE(3) denote the relative pose of the rigid body. The generalized velocity (including rotational angular velocity and translational linear velocity) representing the motion of a rigid body satisfies the following kinematic relationship:

[0045]

[0046] Assuming the generalized velocity ξ is bounded and continuous throughout the estimation process, its measured value Includes the true velocity ξ and time-varying bias Right now:

[0047] ξ y =ξ+b a

[0048] Assuming an inertial frame of reference is known n time-invariant reference vectors i = 1, 2, ..., n, which are in this system The measurable and measured values ​​are expressed as follows:

[0049]

[0050] These n reference vectors contain n1 landmarks and n-n1 inertial vectors, with the following forms: and The measured values ​​of the landmark point and the inertial vector in this system can be expressed as follows:

[0051] as well as

[0052]

[0053] Define the weighted geometric center of all landmarks and its measured value as follows:

[0054]

[0055] Where, α i >0, i = 1, 2, ..., n1, Define the following corrected inertial vector and its measured value:

[0056]

[0057] Where i = 1, 2, ..., n1. Define the set of all inertial vectors (including the original known inertial vectors and the modified inertial vectors). To ensure the observability of the system, it is required that at least one landmark point among the n measurement vectors is measurable, and There are at least two non-collinear inertial vectors.

[0058] Define matrix:

[0059]

[0060] Where k i >0, i=1,2,…n,

[0061] For any g∈SE(3), the following equation holds:

[0062]

[0063]

[0064] The present invention will now be described in detail based on the prior knowledge provided:

[0065] Step S1: Using the reference frame containing the cooperative target as the inertial reference frame, and the reference frame containing the robotic arm platform as the home frame. Let g, b... a These represent the true values ​​of the pose and velocity offsets of the robotic arm platform relative to the cooperative target, respectively. and These are the corresponding estimated values, and the pose estimation error is defined as... The velocity bias estimation error is

[0066] Step S2: First, give the existing classical potential function:

[0067]

[0068] in Information on reference vectors and landmarks on the cooperative target, as well as the gain parameter k, measured by sensors carried on the robotic arm platform. i These are jointly determined and used as process variables in the derivation. The aforementioned potential function is most widely used in pose estimation problems. This invention considers a novel potential function. They are as follows:

[0069]

[0070]

[0071] in, These are coefficients to be determined.

[0072] Step S3: Give The gradient is:

[0073]

[0074] Based on The gradient correction term is designed as follows:

[0075]

[0076]

[0077] Where, k i ,r i ,b i Let i = 1, ..., n represent n self-selected constant gain parameters, n reference feature vectors or landmarks in the cooperative target coordinate system, and the sensor of the robotic arm platform in this system for the relationship between r and r. i The measurement vectors of i = 1, ..., n.

[0078] β1 is the pose correction term, σ b1 This is the correction term for the velocity constant bias. According to the chain rule, it is obtained... The gradients are respectively:

[0079]

[0080]

[0081] based on The gradient correction terms are designed as follows:

[0082]

[0083]

[0084]

[0085]

[0086] Where β2, σ b2 Based on The gradient design includes pose correction terms and velocity constant bias correction terms, correspondingly, β3, σ b3 Based on The gradient design includes pose correction terms and velocity constant bias correction terms, and

[0087]

[0088] Step S4: Introduce different correction terms into the system's kinematic or dynamic equations to obtain the observer:

[0089]

[0090]

[0091] in, They represent The derivative with respect to time, k β Γ>0 is a self-selected constant gain parameter, ξ y The sensors on the robotic arm platform measure speed information. β,σ b , They can be replaced with β1 and σ respectively. b1 β2,σ b2 , and β3,σ b3 , Therefore β,σ b It can be represented as

[0092]

[0093]

[0094] To prove the stability of the observer, the system closed-loop error dynamic equation is established as follows:

[0095]

[0096]

[0097] in They represent The derivative with respect to time. Define the following class of Lyapunov functions on SE(3):

[0098]

[0099] Regarding the time derivative, we obtain:

[0100]

[0101] It is valid if and only if At this point, the observer falls at the unstable critical point; at points outside the unstable critical point, there is... Therefore, the observer is almost globally asymptotically stable.

[0102] To better illustrate the superiority of the observer designed based on this invention, a simulation case of the observer is constructed: Assume that the angular velocity and linear velocity of the robotic arm platform relative to the cooperative target are ω(t) = [-sin(t), cos(t), 0] T v(t) = 2[cos(t),sin(t),0] T The angular velocity and linear velocity biases measured by the velocity sensor are b, respectively. a,ω =cos(0.02t)[-0.02,0.02,0.1] T b a,v =cos(0.02t)[0.2,-0.1,0.01] T , by ω,v,b a,ω ,b a,v Together, velocity measurement information ξ is generated y The invariant reference vector in the cooperative target coordinate system measured by inertial reference vector measurement sensors (including optical cameras measuring landmarks and magnetometers measuring the corresponding vectors of magnetic fields) is... v2 = [0, 0, 1] T v3 = [1, 0, 0] T The corresponding gain parameters are k1 = 1, k2 = 1, k3 = 3, k4 = 1. Based on p1, v1, v2, v3, r1, r2, r3, r4 are generated. Considering the robustness of the observer, it is assumed that the sensor has no measurement noise (which does not affect the validity of the conclusion), that is, b1, b2, b3, b4 are directly generated from r1, r2, r3, r4. Let k... β =1,k ω =1,k v =1, and the initial pose of the robotic arm platform relative to the reference pose is p(0) = [0, 1, 4] T The initial estimate of the observer is (Here, v2 represents the eigenvector of Q). The parameter m is set to 8. Initializing the observer near the unstable critical point on SE(3) is to slow down the convergence of the observer in the initial stage, so that the comparison of the convergence speed of different observers in the initial stage is more visualized, and at the same time, to a certain extent, the estimation error caused by the coupling of attitude and position is avoided from fluctuating significantly.

[0103] Let them be β1 and σ respectively. b1 ,β2,σ b2 and β3,σ b3 The observers for the correction term are Observer I, Observer II, and Observer III. Simulation results are attached. Figure 2-5 As shown in the figure, observers II and III converge faster than observer I, exhibiting better estimation performance. Further simulation experiments indicate that the convergence of observers II and III is related to their stability and the selection of parameter m. When m is much larger than the system's maximum possible error energy, the estimation performance of observers II and III tends to converge with that of observer I. Therefore, m needs to be selected appropriately based on the requirements.

Claims

1. A design method for a cooperative target pose observer based on a novel potential function, characterized in that, Includes the following steps: Step S1: Design an error term that can characterize the estimated pose information and the true pose information of the cooperative target relative to the operating platform. This error term can reflect the deviation between the true pose information and the estimated pose information provided by the observer. Step S2: Design a potential function that is a non-negative, continuously differentiable function with respect to the error state; For any error state, the potential function is always non-negative; The potential function is equal to zero if and only if the error term is a unit value, that is, the estimated state is equal to the true state; Step S3: Calculate the gradient of the potential function, and based on the nonlinear control theory, introduce a correction term from the system's kinematic or dynamic equations. Step S4: Introduce the kinematic or dynamic equations of the system and combine them with nonlinear control theory to ensure that the observer is stable; if the observer is unstable, return to step S2 and redesign the potential function based on the error function. In step S2, the potential function , They are as follows: ； ； in, These are coefficients to be determined; In step S3, the following is given: The gradient is: ； Based on The gradient correction term is designed as follows: ； in, They are respectively A user-selectable constant gain parameter Reference feature vectors or landmarks in the coordinate system of the cooperative target, and the sensors of the robotic arm platform in this system... The measurement vector; For pose correction terms, The correction term for the velocity constant bias is obtained according to the chain rule. , The gradients are respectively: ； ； based on , The gradient correction terms are designed as follows: ； ； in , Based on The gradient design includes pose correction terms and velocity constant bias correction terms, respectively. , Based on The gradient design includes pose correction terms and velocity constant bias correction terms, and 。 2. The design method for a cooperative target pose observer based on a novel potential function according to claim 1, characterized in that: In step S1, let These represent the true values ​​of the pose and velocity offsets of the robotic arm platform relative to the cooperative target, respectively. and These are the corresponding estimated values, and the pose estimation error is defined as... The velocity bias estimation error is .

3. The method for designing a cooperative target pose observer based on a novel potential function according to claim 1, characterized in that: In step S4, different correction terms are introduced into the kinematic or dynamic equations of the system to obtain the observer: ； in, , They represent The derivative with respect to time, For the user-selected constant gain parameter, The sensor on the robotic arm platform measures speed information; , Replace with , , , and , ,therefore Represented as: 。 4. The method for designing a cooperative target pose observer based on a novel potential function according to claim 3, characterized in that: To prove the stability of the observer, the system closed-loop error dynamic equation is established as follows: ； in , They represent The derivative with respect to time; in The following class of Lyapunov functions is defined: ； Regarding the time derivative, we obtain: ； It is valid if and only if At this point, the observer falls at the unstable critical point, and at points outside the unstable critical point, there are... Therefore, the observer is almost globally asymptotically stable.

Images