A two-wheeled vehicle contact force estimation method and device based on first-type lagrange equation

Abstract

The present disclosure relates to the technical field of two-wheeled vehicle systems, and particularly relates to a two-wheeled vehicle contact force estimation method and device based on a first type of Lagrange equation, the method comprising: obtaining a posterior motion state, a control input and a posterior covariance matrix of a two-wheeled vehicle at a current time; determining a first predicted motion state and a predicted covariance matrix at a next time by ignoring the contact force; generating a second predicted motion state at the next time by considering the contact force; obtaining observation data at the next time based on a preset observation function; and solving an optimization objective function under the premise of meeting a preset physical condition to determine a posterior motion state, a wheel-ground contact force and a posterior covariance matrix at the next time. The method provided in the present disclosure introduces the contact force as an independent unknown variable into the equation, constructs a discrete dynamics model containing the wheel-ground contact force, explicitly describes the influence of the contact force on the motion of the two-wheeled vehicle, and improves the estimation accuracy in complex scenarios.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the technical field of two-wheeled vehicle systems, in particular to a two-wheeled vehicle contact force estimation method and device based on a first type of Lagrange equation. BACKGROUND

[0002] Two-wheeled vehicle systems are widely used in the fields of service robots, mobile navigation platforms, etc. due to their simple structure and strong flexibility. High-precision state estimation of two-wheeled vehicles usually relies on Kalman filter for recursive estimation, and accurate state prediction is highly dependent on the accuracy and consistency of the system dynamics model.

[0003] In related technologies, two-wheeled vehicle state estimation is usually based only on control input and kinematic model, resulting in significant state estimation error under non-continuous contact conditions such as uneven ground, slipping or jumping. Therefore, how to additionally consider the dynamic influence of wheel-ground contact in the modeling process of two-wheeled vehicles, so as to improve the accuracy and stability of state estimation under non-continuous contact conditions, has become a problem to be solved. SUMMARY

[0004] Therefore, the present disclosure provides a two-wheeled vehicle contact force estimation method and device based on a first type of Lagrange equation to solve the problem of how to additionally consider the dynamic influence of wheel-ground contact in the modeling process of two-wheeled vehicles, so as to improve the accuracy and stability of state estimation under non-continuous contact conditions.

[0005] In one aspect, the present disclosure provides a two-wheeled vehicle contact force estimation method based on a first type of Lagrange equation, the method comprising: obtaining the posterior motion state, control input and posterior covariance matrix of a two-wheeled vehicle system at a current time; determining the first predicted motion state and predicted covariance matrix of the two-wheeled vehicle system at a next time based on a preset discrete dynamics equation of the two-wheeled vehicle system while ignoring the wheel-ground contact force; superimposing the disturbance of the wheel-ground contact force in the first predicted motion state at the next time to generate the second predicted motion state of the two-wheeled vehicle system at the next time while considering the wheel-ground contact force; obtaining the observation data of the two-wheeled vehicle system at the next time based on a preset observation function, and constructing an optimization objective function at the next time based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state and the observation data; wherein the variables of the observation function and the optimization objective function both include the motion state and the wheel-ground contact force; under the premise of meeting the preset physical conditions, solving the optimization objective function at the next time, determining the posterior motion state and the wheel-ground contact force at the next time corresponding to the optimal solution of the optimization objective function, and performing posterior update on the predicted covariance matrix to determine the posterior covariance matrix at the next time.

[0006] The other aspect of the present disclosure also provides a two-wheeled vehicle contact force estimation device based on the first type of Lagrange equation, the device comprising: a data acquisition module configured to acquire the posterior motion state, control input and posterior covariance matrix of the two-wheeled vehicle system at the current time; a first prior propagation module configured to determine the first predicted motion state and predicted covariance matrix of the two-wheeled vehicle system at the next time based on the preset discrete dynamic equation of the two-wheeled vehicle system while ignoring the wheel-ground contact force; a second prior propagation module configured to superimpose the disturbance of the wheel-ground contact force in the first predicted motion state at the next time to generate the second predicted motion state of the two-wheeled vehicle system at the next time while considering the wheel-ground contact force; an optimization function construction module configured to acquire the observation data of the two-wheeled vehicle system at the next time based on the preset observation function, and construct the optimization objective function at the next time based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state and the observation data; wherein the variables of the observation function and the optimization objective function both include the motion state and the wheel-ground contact force; and an observation update module configured to solve the optimization objective function at the next time under the premise of meeting the preset physical condition, determine the posterior motion state and the wheel-ground contact force at the next time corresponding to the optimal solution of the optimization objective function, and perform posterior update on the predicted covariance matrix to determine the posterior covariance matrix at the next time.

[0007] The other aspect of the present disclosure also provides an electronic device comprising a memory and a processor, the memory and the processor being communicatively connected with each other, the memory storing computer instructions, and the processor executing the computer instructions to perform the two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation.

[0008] The other aspect of the present disclosure also provides a computer readable storage medium, the computer readable storage medium storing computer instructions, the computer instructions being used to make a computer implement the two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation.

[0009] The other aspect of the present disclosure also provides a computer program product comprising computer instructions, the computer instructions being used to make a computer execute the two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation.

[0010] The two-wheeled vehicle contact force estimation method and device based on the first type of Lagrange equation according to the above embodiments of the present disclosure introduce the wheel-ground contact force as an independent unknown variable into the discrete dynamic equation, construct a discrete dynamic model containing the wheel-ground contact force in combination with the first type of Lagrange equation, explicitly describe the influence of the contact force on the motion of the two-wheeled vehicle, and improve the estimation accuracy of the two-wheeled vehicle in complex scenarios. BRIEF DESCRIPTION OF DRAWINGS

[0011] In order to more clearly illustrate the technical solutions in the specific embodiments or the related art of the present disclosure, the drawings needed to be used in the specific embodiments or related art description will be briefly introduced. Obviously, the drawings in the following description are some embodiments of the present disclosure, and other drawings can also be obtained by those skilled in the art without creative labor.

[0012] Figure 1 is a flowchart of a double-wheeled vehicle contact force estimation method based on the first type of Lagrange equation provided by an embodiment of the present disclosure.

[0013] Figure 2 is a specific flowchart of a double-wheeled vehicle contact force estimation method based on the first type of Lagrange equation provided by an embodiment of the present disclosure.

[0014] Figure 3 is a structural schematic diagram of a double-wheeled vehicle contact force estimation device based on the first type of Lagrange equation provided by an embodiment of the present disclosure.

[0015] Figure 4 is a structural schematic diagram of another double-wheeled vehicle contact force estimation device based on the first type of Lagrange equation provided by an embodiment of the present disclosure. DETAILED DESCRIPTION

[0016] The double-wheeled vehicle system is widely used in various scenes such as service robots and autonomous mobile platforms due to its compact structure, flexible motion and low cost, and is especially suitable for mobile tasks in complex indoor and outdoor environments. With the development of autonomous navigation and intelligent control technology, higher requirements are put forward for the state perception and estimation accuracy of the double-wheeled vehicle system. In order to realize real-time perception of the motion state of the double-wheeled vehicle, state estimation algorithms are often used for dynamic reasoning and prediction in the related art, and the extended Kalman filter (EKF) is widely used in the field of robot navigation because it can perform recursive estimation in a nonlinear system. In practical applications, accurate state estimation is highly dependent on the quality of the dynamic modeling of the double-wheeled vehicle.

[0017] The modeling method commonly used in the related art at present is to establish the system's dynamic equation set based on the first type of Lagrange equation. Although this method has theoretical completeness, when facing a double-wheeled vehicle, which is a nonholonomic constraint system, the following problems often exist:

[0018] 1. The double-wheeled vehicle has a non-Euclidean geometric structure, such as the coupled motion of rotation and translation of the whole vehicle, and its motion state is distributed in the Lie group space, making it difficult for the traditional Euclidean space modeling method to accurately express its dynamic evolution process.

[0019] 2. The derivation process of Lagrange equation relies on complicated symbolic computation, involving a large number of intermediate variables (such as generalized coordinates, constraint forces, nonholonomic constraints, etc.), and it is difficult to model, the derivation process is lengthy and prone to symbolic errors in multi-degree-of-freedom scenarios (such as with vehicle body pitch, roll, steering components, etc.).

[0020] 3. The modeling method of the related art lacks structural generality and is not easy to extend to multi-vehicle systems, modular platforms or complex coupled systems, which restricts the scalability and engineering adaptability of the system in higher-level control and collaboration scenarios.

[0021] 4. The state estimation of the related art of the two-wheeled vehicle is usually based on the control input and the kinematic model, resulting in significant state estimation errors under non-flat ground, slipping or jumping, etc. Non-continuous contact conditions.

[0022] To solve the above problems, a two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation is provided in various embodiments of the present disclosure. The method comprises: obtaining the posterior motion state, control input and posterior covariance matrix of the two-wheeled vehicle system at the current time; determining the first predicted motion state and predicted covariance matrix of the two-wheeled vehicle system at the next time based on the preset discrete dynamics equation of the two-wheeled vehicle system under the condition of ignoring the wheel-ground contact force; superimpose the disturbance of the wheel-ground contact force in the first predicted motion state at the next time to generate the second predicted motion state of the two-wheeled vehicle system at the next time under the condition of considering the wheel-ground contact force; based on the preset observation function, obtain the observation data of the two-wheeled vehicle system at the next time, based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state and the observation data, construct the optimization objective function at the next time; wherein the variables of the observation function and the optimization objective function include the motion state and the wheel-ground contact force; under the premise of meeting the preset physical conditions, solve the optimization objective function at the next time, determine the optimal solution of the optimization objective function corresponding to the posterior motion state and the wheel-ground contact force at the next time, and update the predicted covariance matrix to determine the posterior covariance matrix at the next time.

[0023] To make the purpose, technical scheme and advantages of the embodiments of the present disclosure clearer, the technical scheme in the embodiments of the present disclosure will be described clearly and completely below with reference to the drawings in the embodiments of the present disclosure. Obviously, the described embodiments are part of the embodiments of the present disclosure, not all embodiments. Based on the embodiments in the present disclosure, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present disclosure.

[0024] Please refer to Figure 1 , Figure 1This is a flowchart illustrating a method for estimating the contact force of a two-wheeled vehicle based on the first type of Lagrange equation, provided in this disclosure. The method may include the following steps:

[0025] Step S101: Obtain the posterior motion state, control input, and posterior covariance matrix of the two-wheeled vehicle system at the current moment.

[0026] In this embodiment, the current time is used The time period is represented by the next time period. Time indicates.

[0027] Acquiring two-wheeled vehicle systems (hereinafter referred to as two-wheeled vehicles) in Posterior motion state at time 1 Control input With the posterior covariance matrix .

[0028] Among them, control input Specifically, it refers to... The controller constantly applies control signals to the two-wheeled vehicle. For example, control signals may include, but are not limited to, the voltage, current, and torque of the drive motor.

[0029] Posterior covariance matrix at time 1 It can be determined by the posterior update step of the previous round of Kalman filtering, representing the most reliable covariance matrix result of the system at the current moment.

[0030] Step S102: Ignoring wheel-ground contact force, determine the first predicted motion state and predicted covariance matrix of the two-wheeled vehicle system at the next moment based on the preset discrete dynamic equations of the two-wheeled vehicle system.

[0031] In this embodiment, the preset discrete dynamic equation can be used as the state update equation, according to Determine the posterior motion state at time 1. The first predicted motion state at time t. Here, the discrete dynamic equations can be modeled based on Lie groups and their corresponding Lie algebras during the pre-modeling process.

[0032] Furthermore, wheel-ground contact force refers to the force exerted by the ground on the wheel and the force exerted by the wheel on the ground when the two-wheeled vehicle is in contact with the ground during its operation. Wheel-ground contact force can include normal force and tangential force. Normal force is the force perpendicular to the wheel-ground contact surface, generated by the ground supporting the weight of the wheel, and its direction is usually vertically upward. Tangential force is the force parallel to the wheel-ground contact surface and can include friction.

[0033] Ignoring the wheel-ground contact force specifically can mean that the wheel-ground contact force in the preset discrete dynamics equation is first set to 0, and the first predicted motion state at the next moment is calculated under the condition that the wheel-ground contact force is equal to 0 the first predicted motion state at the next moment .

[0034] Here, since the two-wheeled vehicle often includes modeling of internal dynamics and modeling of external interaction in the modeling process of the dynamics equation; by first ignoring the wheel-ground contact force, the modeling process of the two-wheeled vehicle can be divided into internal dynamics prediction and external disturbance correction two parts, thereby separating variables and reducing computational complexity.

[0035] Further, according to the posterior covariance matrix at the next moment and the preset discrete dynamics equation linearly propagates the motion state estimation error at the next moment to determine the prediction covariance matrix . Wherein, linear propagation refers to one of the core ideas in Kalman filtering, that is, the uncertainty of the state at the current moment is linearly transformed through the Jacobian matrix of the model, and then propagated to the next moment. Here, the prediction covariance matrix is used to represent the reliability of the first predicted motion state , that is, to describe the degree to which the first predicted motion state deviates from the true motion state.

[0036] For example, assuming that the current error is , and using a nonlinear function to predict the state at the next moment: In the process of determining the error at the next moment, the nonlinear function is first-order Taylor expanded at the current estimation point: ; wherein is the Jacobian matrix corresponding to the state propagation function; that is, although the function is a nonlinear function, the change of the error can be approximately represented by a linear change , that is, .

[0037] Step S103, in the first predicted motion state at the next moment, superimpose the disturbance of the wheel-ground contact force to generate the second predicted motion state of the two-wheeled vehicle system at the next moment.

[0038] In this embodiment, after calculating the first predicted motion state at the next moment , the wheel-ground contact force is added to the first predicted motion state as the first predicted motion state at the next moment external disturbance correction, further calculating a second predicted motion state at the next time instant .

[0039] At step S104, observation data of the two-wheeled vehicle system at the next time instant is obtained based on a preset observation function, and an optimization objective function at the next time instant is constructed based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state and the observation data.

[0040] In the embodiment, the variables of the observation function and the optimization objective function both include the motion state and the wheel-ground contact force. The observation function represents a mapping relationship from the motion state and the wheel-ground contact force to a sensor observable space.

[0041] Here, the variables of the observation function and the optimization objective function both include the motion state and the wheel-ground contact force, representing that the wheel-ground contact force is taken as an explicit unknown variable of the observation function and the optimization objective function.

[0042] Further, the observation function is used to obtain observation data of the two-wheeled vehicle at the next time instant by using a sensor, and preprocessing such as denoising and time synchronization is performed. The first predicted motion state and the wheel-ground contact force = 0 are input to the observation function to determine a predicted observation value.

[0043] Still further, based on the first predicted motion state , the predicted covariance matrix , the second predicted motion state , the observation data and the predicted observation value, an optimization objective function at the next time instant is constructed.

[0044] Here, the optimization objective function can be used to jointly estimate the motion state and the contact force, and can include a prior consistency term and an observation consistency term. The prior consistency term is used to measure the deviation between the posterior motion state and the first predicted motion state, and the observation consistency term is used to measure the deviation between the observation data and the predicted observation value.

[0045] At step S105, the optimization objective function at the next time instant is solved under the premise of satisfying a preset physical condition, the posterior motion state and the wheel-ground contact force at the next time instant corresponding to the optimal solution of the optimization objective function are determined, and the predicted covariance matrix is updated to determine a posterior covariance matrix at the next time instant.

[0046] In the embodiment, a preset physical condition constraint is set for the optimization objective function, so that the wheel-ground contact force belongs to the optimal solution of the optimization objective function at the same time, and conforms to the real physical law.

[0047] In the embodiment, the posterior motion state and the wheel-ground contact force at the next time instant corresponding to the optimal solution of the optimization objective function are determined​ Post-moment state Contact force between wheels and the ground Next, calculate the Kalman gain. And the Kalman filter formula is used to predict the covariance matrix. Update and confirm. Posterior covariance matrix at time 1 .

[0048] Here, Kalman gain It is a core parameter in Kalman filtering, used to perform a weighted average between predicted and observed values, thereby achieving the optimal estimate of the system state.

[0049] Furthermore, in After continuously updating the posterior covariance matrix of the motion state, wheel-ground contact force, and covariance matrix, the resulting posterior covariance matrix is... Wheel-to-ground contact force With the posterior covariance matrix As the next The a priori propagation starting point of time is then used to predict new states of motion. Wheel-to-ground contact force With covariance matrix This enables step-by-step state estimation and contact force estimation.

[0050] The two-wheeled vehicle contact force estimation method and apparatus based on the first-type Lagrange equation, as described in the above embodiments of this disclosure, introduces the wheel-ground contact force as an independent unknown variable into the discrete dynamic equation. By combining this with the first-type Lagrange equation, a discrete dynamic model containing the wheel-ground contact force is constructed, explicitly describing the influence of the contact force on the motion of the two-wheeled vehicle and improving the estimation accuracy of the two-wheeled vehicle in complex scenarios. Through a prediction-update Kalman filter framework, the system state is forward-propagated at each moment through the dynamic equation, control input, and wheel-ground contact force, and then backward-corrected using observation data and the observation model. This achieves recursive estimation of the system state and wheel-ground contact force over continuous time, significantly improving the accuracy and stability of state estimation under discontinuous contact conditions. By decoupling the functional modules of state modeling, dynamic acquisition, and filtering inference, any part can be replaced or extended. For example, the state space can be extended to complex scenarios such as multi-vehicle systems or legged robots, thereby improving versatility and engineering deployment adaptability.

[0051] In one possible implementation of the above embodiments, before step S101, the method further includes:

[0052] The two-wheeled vehicle system is modeled based on Lie groups and Lie algebras, and the degrees of freedom of the two-wheeled vehicle system are determined to include six degrees of freedom of the floating basis and n joint degrees of freedom.

[0053] exist Defining the pose of a two-wheeled vehicle system in Lie group space In the corresponding Define the floating base velocity of the two-wheeled vehicle system in Lie algebra space. And in combination with joint angles With joint angular velocity To construct the motion state of the two-wheeled vehicle system ;

[0054] Where the number of joint degrees of freedom n in the two-wheeled vehicle system is equal to 3, the motion state of the two-wheeled vehicle system is... The formula is:

[0055]

[0056] Among them, position Belongs to the SE(3) Lie group space, floating basis velocity Belongs to the se(3) Lie algebra space, joint angle With joint angular velocity Both belong to three-dimensional Euclidean space;

[0057] The Pinocchio dynamics library is used to obtain multiple dynamic parameter matrices of the two-wheeled vehicle system in real time. The dynamic parameter matrices include the system mass matrix. Control input matrix Contact force Jacobian matrix Coriolis force vector With the generalized gravity vector ;

[0058] By combining the first type of Lagrange equations, a preset discrete dynamic equation for the two-wheeled vehicle system is constructed;

[0059] The formula for the preset discrete dynamic equations is as follows:

[0060]

[0061] in, For time steps, For time step; , , and They are respectively Position at any moment Joint angle Floating base velocity With joint angular velocity ; , , with respectively pose at time joint angle floating base velocity and joint angular velocity ; is a composition operator on Lie group, is an exponential map; is the inverse matrix of system mass matrix , is the control input of the two-wheeled vehicle system at time , is the wheel-ground contact force as an unknown variable; represents the first 6 rows of elements, corresponding to the 6 degrees of freedom of the floating base, represents the last 3 rows of elements, corresponding to the 3 degrees of freedom of the joint; is a state transition formula, used to describe the state evolution of the two-wheeled vehicle system from time to time ;

[0062] An observation function containing motion state and wheel-ground contact force is constructed.

[0063] In this embodiment, the Lie group can refer to a continuous group with a smooth manifold structure, whose elements can be represented as matrices or transformations, and the group operation (multiplication, inversion) and the manifold structure are compatible. For example, the Lie group can include but is not limited to: group, group, etc.; wherein, group represents the pose (i.e. position and attitude) of a rigid body in three-dimensional space group represents the rotation of a rigid body in three-dimensional space.

[0064] Correspondingly, the Lie algebra can refer to the tangent space of the Lie group at the identity element, whose elements can be matrices or vectors, used to describe the local change of the Lie group.

[0065] Further, the Lie algebra element can be converted into the Lie group element by using the exponential map Exp( ), and the Lie group element can be converted into the Lie algebra element by using the logarithmic map Log( ).

[0066] Here, the bidirectional conversion of Lie algebra to Lie group can be realized by using the exponential map and the logarithmic map, and the mapping is smooth and unique in the global range, so as to avoid the singularity and consistency risk of using Euler angle modeling in related technologies.

[0067] Further, the pose of the two-wheeled vehicle can include a position and an attitude, where the position can represent a spatial position of a reference point of the two-wheeled vehicle in a world coordinate system; and the attitude can describe an orientation of the two-wheeled vehicle relative to the world coordinate system.

[0068] For example, the pose of the two-wheeled vehicle can be represented as Herein, is a 3x3 rotation matrix for representing the attitude of the two-wheeled vehicle rigid body; is a three-dimensional translation vector for representing the position of the two-wheeled vehicle rigid body; and the last row of the matrix is [0, 0, 0, 1] for ensuring the closure of the group operation.

[0069] The joint angles of the two-wheeled vehicle refer to the rotation angles of each movable joint of the two-wheeled vehicle, for representing the configuration of the mechanical structure of the two-wheeled vehicle. Herein, the two-wheeled vehicle can have n joints.

[0070] The floating-base velocity of the two-wheeled vehicle refers to the linear velocity and angular velocity of the floating base of the two-wheeled vehicle in space, where the linear velocity represents the translation speed of the center of mass of the floating base in the world coordinate system, and the angular velocity represents the rotation speed of the floating base around each coordinate axis. Herein, the floating base refers to a base body in the two-wheeled vehicle that is not fixed to the ground, for example, the floating base can refer to the main body of the vehicle body of the two-wheeled vehicle.

[0071] The joint angular velocity of the two-wheeled vehicle refers to the derivative of each joint angle with respect to time, for representing the speed and direction of joint motion.

[0072] In one possible implementation, the Lie group is used to represent the pose of the two-wheeled vehicle, and the Lie group corresponds to the Lie algebra is used to represent the floating-base velocity of the two-wheeled vehicle, and the joint angles and joint angular velocities are combined to construct the motion state of the two-wheeled vehicle system.

[0073] Herein, the Lie group As a rigid body transformation group of three-dimensional Euclidean space, it naturally includes rotation and translation, without the need to decompose the attitude into multiple angles as in the Euler angles of the related art, avoiding the singularity caused by decomposition; at the same time, the Lie algebra directly corresponds to the linear velocity and angular velocity of the rigid body, and its exponential mapping can guarantee the geometric correctness of the update from the velocity to the pose. In addition, the differential operation on the Lie group can naturally handle the nonholonomic constraint, without the need to manually derive the constraint equation as in the related art, greatly reducing the modeling complexity.

[0074] The floating base in the two-wheeled vehicle is connected to other components through joints, and each independent joint can provide one degree of joint freedom; when there are n joints in the two-wheeled vehicle, the two-wheeled vehicle has six degrees of freedom of the floating base and n degrees of joint freedom.

[0075] In the above embodiments of the present disclosure, in order to facilitate description and illustration, the following is described by taking a two-wheeled vehicle with three joint degrees of freedom (i.e., n = 3) as an example. It should be understood that the method in the embodiments is not limited to n = 3, and is still applicable to the case where n takes any non-negative integer; that is, when the number of joint degrees of freedom n takes different values, the method is still applicable, and does not affect the applicability and effectiveness of the modeling structure and state estimation algorithm.

[0076] It can be further understood that when n takes any non-negative integer, the motion state X = (Xv, Xq) constructed is In addition, the generalized velocity of the two-wheeled vehicle is composed of the floating base velocity and the joint angular velocity, and the total dimension of the generalized velocity is 6 + n, that is, the sum of the six degrees of freedom of the floating base and the n joint degrees of freedom.

[0077] Joint angle of the two-wheeled vehicle Here may be a 3 × 1 column vector representing the angles of the three joints in the two-wheeled vehicle; the joint angular velocity of the two-wheeled vehicle Here represents the angular velocity of the three joints in the two-wheeled vehicle.

[0078] The Pinocchio dynamics library can be a robot dynamics algorithm library based on C++ or Python, which is specially designed for fast calculation of dynamics parameters of multi-body systems.

[0079] Specifically, the Pinocchio dynamics library is based on Lie group as a mathematical architecture, which can quickly calculate dynamics parameters through efficient algorithms, and can quickly calculate complex derivatives through built-in automatic differentiation tools.

[0080] It can be understood that the Pinocchio dynamics library can be used to quickly model the rigid body structure of the two-wheeled vehicle, and can also obtain multiple dynamics parameter matrices of the two-wheeled vehicle system in real time.

[0081] Here, modeling the rigid body structure of the two-wheeled vehicle can include defining the connecting rods, joint types, and connection relationships of the two-wheeled vehicle, etc.

[0082] Further, the multiple dynamics parameter matrices obtained in real time by the Pinocchio dynamics library can include dynamics parameters of the first type of Lagrange equation.

[0083] Here, the first type of Lagrange equation is a basic equation for describing the dynamics of a multi-body system in analytical mechanics, which can be used to process the nonholonomic constraints of the two-wheeled vehicle by introducing constraint forces to process the constraints in the system.

[0084] Furthermore, by using multiple dynamic parameter matrices and the first kind of Lagrange equations, the continuous dynamic equations are transformed into pre-defined discrete dynamic equations.

[0085] Specifically, input two-wheeled vehicle in Given the current motion state at any given moment, the system mass matrix of the two-wheeled vehicle is calculated in real time by calling the dynamics function interface of the Pinocchio dynamics library. Control input matrix Contact force Jacobian matrix Coriolis force vector With the generalized gravity vector As a component of the first kind of Lagrange equations.

[0086] Among them, the system quality matrix This refers to the description of the inertial properties of the system's generalized coordinates, used to characterize the inertial coupling relationship between the system's degrees of freedom; control input matrix. Used to describe the mapping relationship between control inputs and generalized coordinates; contact force Jacobian matrix Used to describe the linear relationship between the contact point linear velocity and the generalized velocity; Coriolis force vector A term describing the combined centrifugal and Coriolis forces generated under nonlinear motion of a system; generalized gravity vector. Used to describe the distributed force in a generalized coordinate system when a system is subjected to a gravitational field.

[0087] Furthermore, in the discrete dynamic equations of the constructed two-wheeled vehicle system, the two-wheeled vehicle... floating base velocity at time Here, a two-wheeled vehicle is... The floating base velocity at time t is The net force acting on the object in the current state is ,pass The net force is converted into acceleration, and then... Perform Euler integrals to update the velocity. Since we only consider the velocity portion of the first 6 dimensions of the floating basis, we take the first 6 dimensions of the updated result to determine the two-wheeled vehicle's position. floating base velocity at time .

[0088] Two-wheeled vehicle Joint angular velocity at time t .

[0089] Here, similarly, two-wheeled vehicles are... The joint angular velocity at time t is The net force acting on the object in the current state is ,pass The net force is converted into acceleration, and then... Perform Euler integrals to update the velocity. Since we only focus on the velocity components of the joints in the last three dimensions, we take the last three dimensions of the updated result to determine the two-wheeled vehicle's position. Joint angular velocity at time t .

[0090] Furthermore, two-wheeled vehicles in Position at any moment .

[0091] Here, in determining the two-wheeled vehicle floating base velocity at time Afterwards, floating base velocity at time Perform an exponential mapping to obtain the pose increment, and then correlate it with the two-wheeled vehicle's position. Position at any moment Perform left multiplication, that is, in Position at any moment Applying pose increments, we obtain the position of the two-wheeled vehicle in... Position at any moment .

[0092] Two-wheeled vehicle Joint angle at time .

[0093] Here, in determining the two-wheeled vehicle Joint angular velocity at time t Afterwards, Joint angular velocity at time t Multiply by the time length to determine the joint angle increment, and add the joint angle increment to the two-wheeled vehicle. Joint angle at time In the middle, get in Joint angle at time .

[0094] Constructing a system containing motion states Contact force between wheels and the ground observation function The observation data is obtained from the two-wheeled vehicle based on the observation function; the observation data includes the motion state data and wheel-ground contact force data of the two-wheeled vehicle.

[0095] The two-wheeled vehicle contact force estimation method and apparatus based on the first type of Lagrange equation, as described in the above embodiments of this disclosure, by introducing... With the corresponding A floating base state model for a two-wheeled vehicle is constructed, establishing a coordinate-independent and geometrically consistent representation of its motion state, avoiding singularity issues and making the modeling more rigorous. The motion state is universal across n-dimensional joint degrees of freedom, improving the versatility and scalability of the modeling method, thus adapting it to two-wheeled vehicles with different configurations and degrees of freedom. Lie group modeling naturally considers nonholonomic constraints and kinematic consistency, eliminating the need for explicit construction or solution of constraint equations in Euclidean space, reducing the tedious process and error risk in model derivation, and improving the stability and reliability of the state estimation system.

[0096] In one possible implementation of step S101 above, obtaining the posterior motion state, control input, and posterior covariance matrix of the two-wheeled vehicle system at the current moment includes:

[0097] Obtain the posterior motion state of the two-wheeled vehicle system at the current moment. Control input With the posterior covariance matrix ;

[0098] Ignoring wheel-ground contact forces, the first predicted motion state and predicted covariance matrix of the two-wheeled vehicle system at the next moment are determined based on the preset discrete dynamic equations of the two-wheeled vehicle system, including:

[0099] Ignoring wheel-ground contact force In this case, the first predicted motion state of the two-wheeled vehicle system at the next moment is determined based on the discrete dynamic equations. ;

[0100] Among them, the first predicted motion state of the two-wheeled vehicle system in the next moment. The formula is:

[0101]

[0102] Among them, wheel-to-ground contact force Set to 0;

[0103] Obtain the posterior motion state at the current moment. The corresponding posterior covariance matrix With process noise covariance matrix ;

[0104] According to the state transition formula Regarding the state of motion The partial derivatives determine the state transition Jacobian matrix. ;

[0105] Based on the posterior covariance matrix at the current time Process noise covariance matrix With the state transition Jacobian matrix determining a predicted motion state at a next time instant corresponding to the predicted motion state ;

[0106] wherein the predicted covariance matrix at the next time instant is given by:

[0107]

[0108] wherein is a transpose of a state transition Jacobian matrix , = .

[0109] In the present embodiment, the first predicted motion state is made free from the disturbance of the wheel-ground contact force by setting the wheel-ground contact force to 0.

[0110] In the first predicted motion state , .

[0111] Here, the floating base velocity of the two-wheeled vehicle at the time instant is , the resultant force received at the current state is , the resultant force is converted into an acceleration by , and an Euler integration is performed using to update the velocity. Since only the floating base velocity part of the first 6 dimensions is concerned, the first 6 dimensions of the updated result are taken to determine the floating base velocity of the two-wheeled vehicle at the time instant .

[0112] The joint angular velocity of the two-wheeled vehicle at the time instant .

[0113] Here, similarly, the joint angular velocity of the two-wheeled vehicle at the time instant is , the resultant force received at the current state is , the resultant force is converted into an acceleration by , and an Euler integration is performed using to update the velocity. Since only the joint velocity part of the last 3 dimensions is concerned, the last 3 dimensions of the updated result are taken to determine the joint angular velocity of the two-wheeled vehicle at the time instant .

[0114] Further, the joint angular acceleration of the two-wheeled vehicle at the time instant ​The pose at the time point .

[0115] Here, after determining the joint angular velocity of the two-wheeled vehicle at the time point , the joint angular velocity of the two-wheeled vehicle at the time point is exponentially mapped to obtain a pose increment, and the pose of the two-wheeled vehicle at the time point is multiplied by the pose increment on the pose of the two-wheeled vehicle at the time point to obtain the pose of the two-wheeled vehicle at the time point . . Here, after determining the joint angular velocity of the two-wheeled vehicle at the time point , the joint angular velocity of the two-wheeled vehicle at the time point is multiplied by the time length to determine a joint angular increment, and the joint angular increment is added to the joint angle of the two-wheeled vehicle at the time point to obtain the joint angle of the two-wheeled vehicle at the time point

[0116] . .

[0117] Here, after determining the joint angular velocity of the two-wheeled vehicle at the time point , the joint angular velocity of the two-wheeled vehicle at the time point is multiplied by the time length to determine a joint angular increment, and the joint angular increment is added to the joint angle of the two-wheeled vehicle at the time point to obtain the joint angle of the two-wheeled vehicle at the time point . Further, the process noise covariance matrix at the time point is obtained. The process noise covariance matrix is used to represent modeling errors and random disturbances in the dynamic evolution process of the preset discrete dynamic equation. For example, the errors and disturbances here can include but are not limited to: motor control errors, external disturbances, residual errors of an approximate model, etc.

[0118] Still further, the partial derivative of the state transition formula is taken, and the first-order linearization approximation of the nonlinear state transition formula is performed at the point to construct a state transition Jacobian matrix

[0119] = . In one possible implementation, the expansion form of the state transition Jacobian matrix is:

[0120] The formula of is:

[0121]

[0122] ​​​​

[0123] wherein, x(k) represents the a posteriori motion state of the two-wheeled vehicle system at the current time instant k x(k+1) represents the predicted motion state of the two-wheeled vehicle system at the next time instant k+1 the linearized effect of the first partial derivative matrix is the adjoint operator in the Lie group, represents the adjoint operator acting on the inverse of the exponential map; is the right Jacobian matrix; represents the first partial derivative matrix of the acceleration with respect to the generalized coordinates; is the identity matrix; represents the 6x6 submatrix of the first partial derivative matrix consisting of the first 6 rows and the first 6 columns; represents the 6x3 submatrix of the first partial derivative matrix consisting of the first 6 rows and the last 3 columns; represents the 3x6 submatrix of the first partial derivative matrix consisting of the last 3 rows and the first 6 columns; represents the 3x3 submatrix of the first partial derivative matrix consisting of the last 3 rows and the last 3 columns;

[0124] is given by:

[0125]

[0126] wherein, u(k) represents the control input of the two-wheeled vehicle system at the current time instant k x(k+1) represents the predicted motion state of the two-wheeled vehicle system at the next time instant k+1 the linearized effect of the second partial derivative matrix represents the second partial derivative matrix of the acceleration with respect to the generalized velocity; represents the 6x6 submatrix of the second partial derivative matrix consisting of the first 6 rows and the first 6 columns; represents the 6x3 submatrix of the second partial derivative matrix consisting of the first 6 rows and the last 3 columns; represents the 3x6 submatrix of the second partial derivative matrix consisting of the last 3 rows and the first 6 columns; represents the 3x3 submatrix of the second partial derivative matrix consisting of the last 3 rows and the last 3 columns; wherein the first partial derivative matrix and the second partial derivative matrix are obtained in real-time by the Pinocchio dynamics library;

[0127] is given by:

[0128] wherein, x(k) represents the a posteriori motion state of the two-wheeled vehicle system at the current time instant k a(k+1) represents the predicted generalized acceleration of the two-wheeled vehicle system at the next time instant k+1 the linearized effect of the first partial derivative matrix

[0129] The formula is:

[0130] in, Generalized velocity used to characterize the two-wheeled vehicle system at the current moment. Predicting generalized acceleration at the next moment The linearization effect; generalized velocity The dimension is equal to the sum of the six dimensions of the floating base and the three dimensions of the joint.

[0131] In this embodiment, for ease of description, The top-left sub-block in the corresponding formula use This indicates the top right corner sub-block. use This indicates the bottom left child block. use This indicates, and the lower right corner sub-block use express.

[0132] That is, The formula can be simplified as follows: .

[0133] Among them, sub-block A 6×6 submatrix is ​​used to represent the effect of the floating base state on its own position evolution; sub-block A 6×3 submatrix is ​​used to represent the effect of joint angles on the propagation of floating base states; sub-blocks A 3×6 submatrix is ​​used to represent the effect of floating basis perturbations on the evolution of joint angles; sub-blocks It is a 3×3 submatrix used to represent the effect of joint angle on its own propagation.

[0134] For example, sub-block In Let be the perturbation transformation term on the Lie group, representing the floating basis in How to deal with disturbances from Evolved to ; sub-block In This indicates that in the linear acceleration optimization model, the second-order response of the floating basis element to its own position change (i.e., the partial derivative of acceleration with respect to pose change) is transformed to the Lie algebra space via a right Jacobian transformation and then mapped back to the group space. Therefore, the sub-block It is used to represent the effect of the floating base state on its own position evolution, that is, how the pose at the next moment is amplified or reduced by the current perturbation.

[0135] Furthermore, The upper left sub-block in the corresponding formula is denoted by , the upper right sub-block is denoted by , the lower left sub-block is denoted by , the lower right sub-block is denoted by , and the lower right sub-block is denoted by . That is,

[0136] The formula of can be simplified as: .

[0137] wherein the sub-block is a 6x6 sub-matrix, used for representing the influence of the floating base velocity on the evolution of the own pose; the sub-block is a 6x3 sub-matrix, used for representing the influence of the joint angular velocity on the evolution of the floating base pose; the sub-block is a 3x6 sub-matrix, used for representing the influence of the floating base velocity on the evolution of the joint angle; and the sub-block is a 3x3 sub-matrix, used for representing the influence of the joint angular velocity on the evolution of the joint angle.

[0138] In one possible implementation manner, and can be calculated in real time by using the automatic differentiation tool of the Pinocchio dynamics library.

[0139] Here, by linearizing and expanding the pose and velocity disturbances in the state propagation process respectively, and by using the Lie group / Lie algebra tool to propagate the disturbances in the Lie group space, the state estimation accuracy under is effectively improved. By using the automatic differentiation interface provided by the Pinocchio dynamics library, the partial derivatives of the acceleration with respect to the generalized position and velocity are obtained in real time, without the need for manually deriving complex expressions, thereby improving the engineering implementation efficiency and enhancing the real-time deployability and cross-platform compatibility of the system.

[0140] Based on the state transition Jacobian matrix , the covariance is propagated, and the prediction covariance matrix is calculated.

[0141] wherein is used for linearly propagating the uncertainty at the current time to the next time through the state propagation function; is used for accumulating the newly added noise and uncertainty in the dynamics process into the prediction covariance matrix.

[0142] By the two-wheeled vehicle contact force estimation method and device based on the first type of Lagrange equation according to the above embodiments of the present disclosure, the wheel-ground contact force ​Setting it to 0 eliminates contact force interference and yields an independent prior prediction state. This is achieved by introducing a process noise covariance matrix. As a perturbation model in a system process, it can reflect the influence of multiple uncertain factors, thereby improving the robustness and reliability of the overall state estimation and ensuring the geometric continuity and stability of the state propagation process.

[0143] In one possible implementation of step S103 above, considering the wheel-ground contact force, a disturbance of the wheel-ground contact force is superimposed on the first predicted motion state at the next moment to generate the second predicted motion state of the two-wheeled vehicle system at the next moment, including:

[0144] Considering wheel-to-ground contact force In this case, obtain the wheel-to-ground contact force at the current moment. Predicting the motion state at the next moment The wheel-ground contact force at the current moment is superimposed. Determine the second predicted motion state at the next moment. ;

[0145] Among them, the second predicted motion state at the next moment The formula is:

[0146]

[0147] Among them, the first predicted motion state at the next moment The wheel-ground contact force at the current moment is superimposed. The perturbation generates the second predicted motion state for the next moment. .

[0148] In this embodiment, obtain Wheel-to-ground contact force at any moment Among them, wheel-to-ground contact force To The objective function at each time step is determined by solving the optimization objective function.

[0149] Furthermore, Second predicted motion state at time 1 In the formula, .

[0150] Here, the contact force between the wheel and the ground pass Converted to acceleration and squared over time After integration, pose increments are generated through exponential mapping and superimposed onto the ideal pose without considering wheel-ground contact forces. In the process, the actual position and posture after considering the wheel-ground contact force are determined. .

[0151] second predicted motion state at time In the formula, .

[0152] Here, the wheel-ground contact force is converted to acceleration, and integrated over time squared to get a position increment which is added to the ideal joint angle that does not take into account the wheel-ground contact force to determine the actual joint angle that takes into account the wheel-ground contact force .

[0153] second predicted motion state at time In the formula, .

[0154] Here, the wheel-ground contact force is converted to acceleration, and integrated over time squared to get a velocity increment Since only the first 6 dimensions of the floating base velocity part are of interest, the first 6 dimensions of the updated result are taken, and added to the floating base velocity that does not take into account the wheel-ground contact force to determine the actual floating base velocity that takes into account the wheel-ground contact force .

[0155] second predicted motion state at time In the formula, .

[0156] Here, the wheel-ground contact force is converted to acceleration, and integrated over time squared to get a velocity increment Since only the last 3 dimensions of the floating base velocity part are of interest, the last 3 dimensions of the updated result are taken, and added to the joint angular velocity that does not take into account the wheel-ground contact force to determine the actual joint angular velocity that takes into account the wheel-ground contact force .

[0157] The two-wheeled vehicle contact force estimation method and device based on the first type of Lagrange equation according to the above-mentioned embodiments of the present disclosure, by first calculating the first predicted state without contact force disturbance, and then explicitly superimposing the contact force disturbance in a physically consistent manner to form the second predicted state, realizes the explainable modeling of the state propagation process for the wheel-ground contact force, and improves the controllability of the state estimation. The wheel-ground contact force is converted to acceleration through the mass matrix ​The mapping is generalized acceleration, and the integral is velocity and pose increment, which follows the Lagrange dynamics principle, avoids non-physical direct disturbance interpolation, and improves the physical consistency and robustness of the estimation system.

[0158] In one possible implementation of the above step S104, the observation data of the two-wheeled vehicle system at the next time is obtained based on a preset observation function, and an optimization objective function at the next time is constructed based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state, and the observation data, including:

[0159] The observation data of the two-wheeled vehicle system at the next time is obtained based on a preset observation function .

[0160] The optimization objective function is constructed based on the first predicted motion state at the next time, the predicted covariance matrix , the second predicted motion state , and the observation data .

[0161] The formula of the optimization objective function is:

[0162]

[0163] wherein, is the posterior motion state at the next time, is the wheel-ground contact force at the next time; represents a first-order partial derivative matrix of the observation function with respect to the motion state ; represents a first-order partial derivative matrix of the observation function with respect to the wheel-ground contact force ; represents a correction amount of the posterior motion state with respect to the first predicted motion state; , represents the predicted observation data in the case of ignoring the wheel-ground contact force ;

[0164] The optimization objective function at the next time is solved under the premise of meeting a preset physical condition, and the posterior motion state at the next time and the wheel-ground contact force corresponding to the optimal solution of the optimization objective function are determined, including:

[0165] The optimization objective function at the next time is solved under the premise of meeting a preset physical condition, and the posterior motion state at the next time and the wheel-ground contact force corresponding to the optimal solution of the optimization objective function are determined. ​

[0166] In this embodiment, the two-wheeled vehicle is obtained from the sensors. Actual observation data at time The predicted observation data were calculated by neglecting wheel-ground contact force. ;

[0167] Calculate the observation residuals as well as .

[0168] according to Solving the objective function at time step to find the optimal solution Posterior motion state at time 1 Contact force between wheels and the ground .

[0169] Here, the optimal solution is obtained by optimizing the objective function. Posterior motion state at time 1 Contact force between wheels and the ground The following conditions must be met: posterior motion state Near the second predicted motion state, while minimizing the observation error, the posterior motion state is... Contact force between wheels and the ground The projection in the observation space should be as close as possible to the actual observed value.

[0170] The two-wheeled vehicle contact force estimation method and apparatus based on the first type of Lagrange equations in the above embodiments of this disclosure use the second predicted state after considering contact force disturbance as the reference center for estimation, and introduces the prediction covariance matrix of the first predicted state as a state constraint. Regularization of the estimation through state residual terms effectively prevents state estimation drift and improves estimation convergence and robustness. By introducing wheel-ground contact force as the variable to be optimized into the state estimation, combined with observation data-driven estimation, online estimation of contact force can be achieved without explicitly installing contact force sensors, solving the practical problem that wheel-ground force cannot be directly measured. Based on a nonlinear observation function... Its locally linearized Jacobian matrix can be flexibly extended to multi-source sensor observation models such as vision and odometer, and has good system scalability and versatility.

[0171] In one possible implementation of step S104 above, the preset physical conditions that the objective function needs to satisfy include: the contact occurrence condition and the linear complementarity condition of the tangential force.

[0172] The formula for the contact occurrence condition is as follows:

[0173]

[0174]

[0175] where, is the normal distance from the wheel to the ground, is the normal contact force experienced by the wheel at the current time instant;

[0176] The formula of the tangential force linear complementarity condition is:

[0177]

[0178]

[0179] where, is the tangent plane basis vector matrix, is the constraint Jacobian matrix, is the generalized velocity vector, is the Lagrange multiplier, is the Coulomb friction coefficient, is an all-ones vector with the same dimension as , is the tangential force coefficient vector, is the complementarity sign.

[0180] In this embodiment, the wheel-ground contact force is decomposed into the normal contact force and the tangential friction force.

[0181] Specifically, the decomposition formula is: .

[0182] where, is the normal unit vector at the contact point, is the scalar of the normal contact force; is the tangential basis matrix, is the linear coefficient of the tangential friction force.

[0183] Further, the physical meaning represented by the formula of the contact occurrence condition is: if the wheel is off the ground ( ), the wheel cannot experience force ( ); if the wheel experiences force ( ), the wheel must be in contact with the ground ( ), ensuring that only when truly in contact is a non-zero contact force allowed to exist.

[0184] Further, by setting to be no less than 0, it is ensured that the wheel will not be lower than the ground, i.e., will not “penetrate” the ground; by setting to be no less than 0, it is ensured that the contact force can only push the object upwards and will not pull the object downwards.

[0185] The optimization objective function is also based on the maximum dissipation principle of tangential friction, and introduces the tangential force condition; the formula of the tangential force condition is:

[0186]

[0187]

[0188] wherein, represents that the selected legal (non-slip) tangential friction force in the current tangential velocity direction makes the system do the minimum work (i.e., maximally resists the tangential motion to achieve maximum energy dissipation); represents that the sum of the tangential friction forces cannot exceed the maximum friction force, otherwise it will slip; is used to constrain the positive direction of the friction coefficient when the orthogonal base is used.

[0189] Here, the maximum dissipation principle means that the friction force is always opposite to the tangential velocity and as large as possible, but still within the Coulomb friction cone constraint.

[0190] Further, based on the Karush-Kuhn-Tucker (KKT) condition, the above tangential force condition is converted into the following linear complementary constraint:

[0191]

[0192]

[0193] wherein, If the tangential friction force component in a certain direction , then the corresponding tangential velocity projection must be 0, which means that the tangential velocity in that direction is zero (no slip), and the friction force is in the static friction state and completely resists the slip. If the tangential velocity projection is greater than 0, then the corresponding tangential friction force is 0, which means that the slip occurs in that direction and the friction force cannot generate additional resistance. If (the friction force has not reached the maximum value), then , which means that the constraint is not active, indicating that the friction force has not reached the limit and is in a static or non-slip state; if (the friction force has reached the maximum value), then , which means that the constraint is active, and the friction force is in the limit state, the system slips, and the friction force is equal to the friction cone boundary.

[0194] Here, the first formula in the linear complementary constraint ensures that the friction force exists when there is no slip, and the friction force is at the limit value and in the direction that resists the slip when there is slip; the second formula The friction force is ensured to be physically reasonable and satisfy the Coulomb friction model. The principle of maximum dissipation is achieved through two formulas, meaning that the power dissipated by friction in the tangential direction is maximized, thus ensuring that the system dynamics satisfy physical laws.

[0195] The two-wheeled vehicle contact force estimation method and apparatus based on the first type of Lagrange equations in the above embodiments of this disclosure ensures that the contact force between the wheel and the ground conforms to physical reality by introducing multiple preset physical conditions into the optimization objective function. Furthermore, it rationally distributes tangential friction force through the maximum dissipation principle, ensuring that the friction force both maximizes resistance to sliding and does not exceed the friction cone limit, thus realizing realistic friction behavior. Simultaneously, it effectively expresses the relationship between friction force and sliding state through linear complementary constraints, facilitating numerical solution and optimization. It exhibits good adaptability to complex motion states such as wheel-ground non-contact, slippage, and jumping, significantly enhancing the estimation accuracy and robustness of the system under varying contact conditions, making it suitable for deployment in wheeled mobile robot systems in real-world complex environments.

[0196] In one possible implementation of step S104 above, the predicted covariance matrix is ​​updated posteriorly to determine the posterior covariance matrix at the next time step, including:

[0197] Constructing Kalman Gain Kalman gain The formula is:

[0198]

[0199] in, To observe the noise covariance matrix, Observation matrix The transpose of the matrix;

[0200] Kalman gain With observation matrix For the prediction covariance matrix Perform a posterior update to determine the posterior covariance matrix of the two-wheeled vehicle system at the next time step. Among them, the posterior covariance matrix of the two-wheeled vehicle system at the next time step. The formula is: .

[0201] In this embodiment, This is used to characterize the uncertainty that has been explained by observations after deducting it from the predicted covariance; that is, after obtaining the observation information, the uncertainty is... Make corrections to form a more reliable posterior covariance matrix. By employing a three-stage structure of prediction-observation-correction, a closed-loop state estimation framework with observation as the core feedback is formed, significantly enhancing the accuracy and robustness of system state estimation.

[0202] In an embodiment, referring to Figure 2 , Figure 2 is a specific flowchart of a two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation provided by the embodiment of the present disclosure, which can include the following steps:

[0203] Step S201, obtaining the posterior motion state, posterior covariance matrix and wheel-ground contact force at the current time;

[0204] Here, the posterior motion state, posterior covariance matrix and wheel-ground contact force at the current time are determined according to the iteration of the posterior update at the last time;

[0205] Step S202, performing prior estimation based on a preset discrete dynamics equation, without considering the wheel-ground contact force, to generate the first predicted motion state and predicted covariance matrix at the next time;

[0206] Step S203, considering the wheel-ground contact force, to continue generating the second predicted motion state at the next time;

[0207] Step S204, obtaining the observation data at the next time;

[0208] Here, the observation data of the two-wheeled vehicle system at the next time is obtained according to the observation function;

[0209] Step S205, based on the observation data, using an optimization objective function to perform posterior update on the predicted motion state, predicted covariance matrix and wheel-ground contact force, to generate the posterior motion state, posterior covariance matrix and wheel-ground contact force at the next time;

[0210] Here, the predicted motion state, predicted covariance matrix and wheel-ground contact force are subjected to a new round of posterior update to generate the posterior motion state, posterior covariance matrix and wheel-ground contact force at the next time, and the iteration at the next time is performed again.

[0211] In an embodiment, a two-wheeled vehicle contact force estimation device 300 based on the first type of Lagrange equation is provided, which corresponds to the two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation in the above embodiment. As Figure 3 shown, the two-wheeled vehicle contact force estimation device 300 based on the first type of Lagrange equation includes a data acquisition module 301, a first prior propagation module 302, a second prior propagation module 303, an optimization function construction module 304 and an observation update module 305, wherein each functional module is described in detail as follows:

[0212] The data acquisition module 301 is configured to acquire the a posteriori motion state, the control input and the a posteriori covariance matrix of the two-wheeled vehicle system at the current time point.

[0213] The first prior propagation module 302 is configured to determine the first predicted motion state and the predicted covariance matrix of the two-wheeled vehicle system at the next time point based on preset discrete dynamic equations of the two-wheeled vehicle system, without considering the wheel-ground contact force.

[0214] The second prior propagation module 303 is configured to superimpose the disturbance of the wheel-ground contact force in the first predicted motion state at the next time point to generate the second predicted motion state of the two-wheeled vehicle system at the next time point, with the wheel-ground contact force being considered.

[0215] The optimization function construction module 304 is configured to acquire observation data of the two-wheeled vehicle system at the next time point based on a preset observation function, and construct an optimization objective function at the next time point based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state and the observation data, wherein the variables of the observation function and the optimization objective function both include the motion state and the wheel-ground contact force.

[0216] The observation update module 305 is configured to solve the optimization objective function at the next time point under the premise of satisfying a preset physical condition, determine the a posteriori motion state and the wheel-ground contact force at the next time point corresponding to the optimal solution of the optimization objective function, and perform a posteriori update on the predicted covariance matrix to determine the a posteriori covariance matrix at the next time point.

[0217] In an embodiment, the two-wheeled vehicle contact force estimation device 300 based on the first type of Lagrange equation further comprises an initialization module 306, wherein:

[0218] The initialization module 306 is configured to

[0219] The two-wheeled vehicle system is modeled based on Lie group and Lie algebra, and the degrees of freedom of the two-wheeled vehicle system include six degrees of freedom of the floating base and n degrees of freedom of the joints.

[0220] In The pose of the two-wheeled vehicle system is defined in the Lie group space , the floating base velocity of the two-wheeled vehicle system is defined in the corresponding Lie algebra space , and the motion state of the two-wheeled vehicle system is constructed in combination with the joint angle and the joint angular velocity . ;

[0221] When the number n of the joint degrees of freedom of the two-wheeled vehicle system is equal to 3, the formula of the motion state of the two-wheeled vehicle system is:

[0222]

[0223] Among them, position Belongs to the SE(3) Lie group space, floating basis velocity Belongs to the se(3) Lie algebra space, joint angle With joint angular velocity Both belong to three-dimensional Euclidean space;

[0224] The Pinocchio dynamics library is used to obtain multiple dynamic parameter matrices of the two-wheeled vehicle system in real time. The dynamic parameter matrices include the system mass matrix. Control input matrix Contact force Jacobian matrix Coriolis force vector With the generalized gravity vector ;

[0225] By combining the first type of Lagrange equations, a preset discrete dynamic equation for the two-wheeled vehicle system is constructed;

[0226] The formula for the preset discrete dynamic equations is as follows:

[0227]

[0228] in, For time steps, For time step; , , and They are respectively Position at any moment Joint angle Floating base velocity With joint angular velocity ; , , and They are respectively Position at any moment Joint angle Floating base velocity With joint angular velocity ; For composition operators on Lie groups, For exponential mapping; System quality matrix The inverse matrix, For two-wheeled vehicle systems Time-based control input, The wheel-ground contact force is an unknown variable. The first six rows of elements correspond to the six degrees of freedom of the floating base, The last three rows of elements correspond to the three degrees of freedom of the joint. is a state transition equation used to describe the state evolution of the two-wheeled vehicle system from time to time .

[0229] An observation function is constructed to include the motion state and the wheel-ground contact force . .

[0230] In an embodiment, the data acquisition module 301 is configured to acquire the posterior motion state

[0231] , the control input and the posterior covariance matrix of the two-wheeled vehicle system at the current time.

[0232] In the case of ignoring the wheel-ground contact force, the first predicted motion state and the predicted covariance matrix of the two-wheeled vehicle system at the next time are determined based on a preset discrete dynamics equation of the two-wheeled vehicle system, comprising:

[0233] In the case of ignoring the wheel-ground contact force , the first predicted motion state of the two-wheeled vehicle system at the next time is determined based on the discrete dynamics equation.

[0234] wherein the formula of the first predicted motion state of the two-wheeled vehicle system at the next time is:

[0235]

[0236] wherein the wheel-ground contact force is set to 0.

[0237] The posterior covariance matrix corresponding to the posterior motion state at the current time and the process noise covariance matrix are acquired.

[0238] The state transition Jacobian matrix is determined according to the partial derivative of the state transition equation with respect to the motion state .

[0239] The predicted motion state at the next time is determined according to the posterior covariance matrix at the current time, the process noise covariance matrix and the state transition Jacobian matrix .​​ the corresponding prediction covariance matrix ;

[0240] wherein the prediction covariance matrix of the next moment is as follows:

[0241]

[0242] wherein, is the transpose matrix of the state transition Jacobian matrix , and the state transition Jacobian matrix = .

[0243] In an embodiment, the second prior propagation module 303 is configured to

[0244] In the case of considering the wheel-ground contact force , the wheel-ground contact force of the current moment is obtained, and the predicted motion state of the next moment is superimposed with the wheel-ground contact force of the current moment, so as to determine the second predicted motion state of the next moment.

[0245] wherein the formula of the second predicted motion state of the next moment is as follows:

[0246]

[0247] wherein the second predicted motion state of the next moment is generated by superimposing the disturbance of the wheel-ground contact force of the current moment on the first predicted motion state of the next moment.

[0248] In an embodiment, the optimization function construction module 304 is configured to

[0249] based on the preset observation function , the observation data of the two-wheeled vehicle system at the next moment is obtained.

[0250] based on the first predicted motion state of the next moment, the prediction covariance matrix , the second predicted motion state and the observation data , an optimization objective function is constructed.

[0251] wherein the formula of the optimization objective function is as follows:

[0252]

[0253] wherein, is a posteriori motion state at next time instant, is a wheel-ground contact force at next time instant; represents a first order partial derivative matrix of the observation function with respect to the motion state ; represents a first order partial derivative matrix of the observation function with respect to the wheel-ground contact force ; represents a correction amount of the a posteriori motion state with respect to the first predicted motion state; , represents predicted observation data in the case of ignoring the wheel-ground contact force ;

[0254] solving the optimization objective function at next time instant under the premise of meeting preset physical conditions, determining the a posteriori motion state and the wheel-ground contact force at next time instant corresponding to the optimal solution of the optimization objective function, comprising:

[0255] solving the optimization objective function at next time instant under the premise of meeting preset physical conditions, determining the a posteriori motion state at next time instant corresponding to the optimal solution of the optimization objective function .

[0256] In an embodiment,

[0257] the preset physical conditions required by the optimization objective function include a contact occurrence condition and a tangential force linear complementarity condition;

[0258] wherein, the formula of the contact occurrence condition is:

[0259]

[0260]

[0261] wherein, is a normal distance of the wheel from the ground, is a normal contact force received by the wheel at the current time instant;

[0262] the formula of the tangential force linear complementarity condition is:

[0263]

[0264]

[0265] wherein, is a tangent plane base vector matrix, To constrain the Jacobian matrix, For generalized velocity vectors, For Lagrange multipliers, Let be the Coulomb friction coefficient. It is a vector of all 1s, with dimensions equal to 1. Consistent, This is the vector of tangential force coefficients. It is a complementary symbol.

[0266] In one embodiment, the observation update module 305 is used for

[0267] Constructing Kalman Gain Kalman gain The formula is:

[0268]

[0269] in, To observe the noise covariance matrix Observation matrix The transpose of the matrix;

[0270] Kalman gain With observation matrix For the prediction covariance matrix Perform a posterior update to determine the posterior covariance matrix of the two-wheeled vehicle system at the next time step. Among them, the posterior covariance matrix of the two-wheeled vehicle system at the next time step. The formula is: .

[0271] It should be noted that the two-wheeled vehicle contact force estimation device based on the first type of Lagrange equation provided in the above embodiments is only illustrated by the division of the above-described program modules when implementing the corresponding two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation. In practical applications, the above processing can be assigned to different program modules as needed, that is, the internal structure of the above system can be divided into different program modules to complete all or part of the processing described above. In addition, the system provided in the above embodiments and the corresponding Figure 1 The embodiments of the methods shown belong to the same concept, and their specific implementation process can be found in the method embodiments, which will not be repeated here.

[0272] This disclosure also provides an electronic device having the above-described features. Figure 3 The device shown is a two-wheeled vehicle contact force estimation device based on the first type of Lagrange equation.

[0273] Please see Figure 4 , Figure 4is a structural schematic diagram of another dual-wheeled vehicle contact force estimation device based on the first type of Lagrange equation provided by the embodiments of the present disclosure, as shown in Figure 4 As shown in FIG. 1, the electronic device includes one or more processors 10, memory 20, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. The various components are communicatively connected using different buses, and can be mounted on a common main board or otherwise mounted as needed. The processor can process instructions executed within the electronic device, including instructions stored in the memory or on the memory to display graphical information of a GUI on an external input / output device, such as a display device coupled to the interface. In some alternative embodiments, multiple processors and / or buses can be used with multiple memories and multiple memory, if desired. Likewise, multiple electronic devices can be connected, each providing part of the necessary operations (e.g., as a server array, a set of blade servers, or a multi-processor system). Figure 4 The processor 10 is taken as an example in the embodiment.

[0274] The processor 10 can be a central processor, a network processor, or a combination thereof. The processor 10 can further include a hardware chip. The hardware chip can be an application specific integrated circuit, a programmable logic device, or a combination thereof. The programmable logic device can be a complex programmable logic device, a field programmable logic device, a general array logic, or any combination thereof.

[0275] The memory 20 stores instructions executable by the at least one processor 10 to cause the at least one processor 10 to perform the methods implemented by the above-mentioned embodiments.

[0276] The memory 20 can include a program storage area and a data storage area. The program storage area can store an operating system and application programs required by at least one function; the data storage area can store data created according to the use of the electronic device, etc. In addition, the memory 20 can include a high-speed random access memory, and can also include a non-transitory memory, such as at least one magnetic disk storage device, a flash memory device, or other non-transitory solid-state memory device. In some alternative embodiments, the memory 20 can optionally include a memory disposed remotely with respect to the processor 10, which can be connected to the electronic device through a network. Examples of the network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof.

[0277] The memory 20 can include a volatile memory, such as a random access memory; the memory can also include a non-volatile memory, such as a flash memory, a hard disk, or a solid state disk; the memory 20 can also include a combination of the above types of memories.

[0278] The electronic device also includes an input device 30 and an output device 40. The processor 10, the memory 20, the input device 30, and the output device 40 can be connected by a bus or other means, Figure 4 The bus connection is taken as an example.

[0279] The input device 30 can receive inputted digital or character information, and generate key signal input related to user settings and function control of the electronic device, such as a touch screen, a keypad, a mouse, a trackpad, a touchpad, a pointing stick, one or more mouse buttons, a trackball, a joystick, etc. The output device 40 can include a display device, an auxiliary lighting device (e.g., an LED), a tactile feedback device (e.g., a vibration motor), etc. The display device includes, but is not limited to, a liquid crystal display, a light emitting diode, a display, and a plasma display. In some alternative embodiments, the display device can be a touch screen.

[0280] The electronic device also includes a communication interface for communication of the electronic device with other devices or communication networks.

[0281] The embodiments of the present disclosure also provide a computer readable storage medium, and the method according to the embodiments of the present disclosure can be implemented in hardware, firmware, or recorded in a storage medium, or be implemented as computer code originally stored in a remote storage medium or a non-transitory machine readable storage medium downloaded through a network and stored in a local storage medium, so that the method described herein can be processed by such software on a storage medium using a general purpose computer, a special purpose processor, or programmable or special purpose hardware. The storage medium can be a magnetic disk, an optical disk, a read-only memory, a random access memory, a flash memory, a hard disk, or a solid state disk, etc. Further, the storage medium can also include a combination of the above-mentioned types of memories. It can be understood that the computer, the processor, the microprocessor controller, or the programmable hardware includes a storage component that can store or receive software or computer code, when the software or computer code is accessed and executed by the computer, the processor, or the hardware, the method shown in the above embodiments is implemented.

[0282] Part of the present disclosure can be applied as a computer program product, for example, computer program instructions, when executed by a computer, through the operation of the computer, the method and / or technical solutions according to the present disclosure can be invoked or provided. Those skilled in the art should understand that the form of computer program instructions in computer readable medium includes but is not limited to source file, executable file, installation package file and the like, and accordingly, the way of computer program instructions executed by computer includes but is not limited to: the computer directly executes the instructions, or the computer compiles the instructions and then executes the corresponding compiled program, or the computer reads and executes the instructions, or the computer reads and installs the instructions and then executes the corresponding installed program. Here, the computer readable medium can be any available computer readable storage medium or communication medium accessible to the computer.

[0283] Although the embodiments of the present disclosure are described in conjunction with the drawings, various modifications and changes can be made by those skilled in the art without departing from the spirit and scope of the present disclosure, and such modifications and changes fall within the scope defined by the appended claims.

Claims

1. A two-wheeled vehicle contact force estimation method based on a first type of Lagrange equation, characterized by, The method comprises: obtaining the posterior motion state, control input and posterior covariance matrix of the two-wheeled vehicle system at the current time; determining the first predicted motion state and predicted covariance matrix of the two-wheeled vehicle system at the next time based on the preset discrete dynamics equation of the two-wheeled vehicle system without considering the wheel-ground contact force; adding the disturbance of the wheel-ground contact force in the first predicted motion state at the next time to generate the second predicted motion state of the two-wheeled vehicle system at the next time under the consideration of the wheel-ground contact force; obtaining the observation data of the two-wheeled vehicle system at the next time based on the preset observation function, and constructing the optimization objective function at the next time based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state and the observation data; wherein the variables of the observation function and the optimization objective function both include motion state and wheel-ground contact force; solving the optimization objective function at the next time under the premise of meeting the preset physical condition, determining the posterior motion state and wheel-ground contact force at the next time corresponding to the optimal solution of the optimization objective function, and performing posterior updating on the predicted covariance matrix to determine the posterior covariance matrix at the next time.

2. The method of claim 1, wherein, Before the step of obtaining the posterior motion state, control input and posterior covariance matrix of the two-wheeled vehicle system at the current time, the method further comprises: modeling the two-wheeled vehicle system based on Lie group and Lie algebra, and determining the degrees of freedom of the two-wheeled vehicle system including six degrees of freedom of the floating base and n joint degrees of freedom; In Defining the pose of a bicycle system in Lie group space , the corresponding Defining the floating-base velocity of a bicycle system in Lie algebra space , and combining joint angles With joint angular velocity , the motion state of the bicycle system is constructed ; wherein, if the number of joint degrees of freedom n in the two-wheeled vehicle system is equal to 3, the motion state of the two-wheeled vehicle system is given by the formula: wherein the pose belongs to the SE(3) Lie group space, the floating base velocity belongs to the se(3) Lie algebra space, the joint angles and the joint angular velocities all belong to the three-dimensional Euclidean space; A Pinocchio dynamics library is employed to obtain in real-time a plurality of dynamics parameter matrices of the two-wheeled vehicle system, including a system mass matrix , a control input matrix , a contact force Jacobian matrix , a Coriolis force vector , and a generalized gravity vector ; constructing the preset discrete dynamics equation of the two-wheeled vehicle system in combination with the first type of Lagrange equation; wherein the formula of the preset discrete dynamics equation is: in, For time steps, For time step; , , and They are respectively Position at any moment Joint angle Floating base velocity With joint angular velocity ; , , and They are respectively Position at any moment Joint angle Floating base velocity With joint angular velocity ; For composition operators on Lie groups, For exponential mapping; System quality matrix The inverse matrix, For two-wheeled vehicle systems Time-based control input, The wheel-ground contact force is an unknown variable. The first six rows of elements represent the six degrees of freedom of the floating basis. The last three rows of elements represent the three degrees of freedom of the joint. This is a state transition formula used to describe the state transition of a two-wheeled vehicle system from... Time's up State evolution at any given moment; constructing a motion state wheel-ground contact force observation function .

3. The method of claim 2, wherein, The step of obtaining the posterior motion state, control input and posterior covariance matrix of the two-wheeled vehicle system at the current time comprises: acquiring a posterior motion state of the two-wheeled vehicle system at a current time , control input and a posterior covariance matrix ; The step of determining the first predicted motion state and predicted covariance matrix of the two-wheeled vehicle system at the next time based on the preset discrete dynamics equation of the two-wheeled vehicle system without considering the wheel-ground contact force comprises: Ignoring wheel-ground contact force In this case, the first predicted motion state of the two-wheeled vehicle system at the next moment is determined based on the discrete dynamic equations. ; wherein the first predicted motion state of the two-wheeled vehicle system at a next time instant is given by the formula: wherein the wheel-ground contact force is set to 0; acquiring a current time instant's posterior motion state the corresponding posterior covariance matrix a process noise covariance matrix ; According to the state transition formula The partial derivative of the motion state determines the state transition Jacobian matrix ; a posteriori covariance matrix for the current time instant , the process noise covariance matrix and the state transition Jacobian matrix , determine a predicted motion state for the next time instant a predicted covariance matrix corresponding where the predicted covariance matrix at the next time instant is given by the formula wherein is the transpose of the state transition Jacobian matrix , the state transition Jacobian matrix = .

4. The method of claim 3, wherein, The step of adding the disturbance of the wheel-ground contact force in the first predicted motion state at the next time to generate the second predicted motion state of the two-wheeled vehicle system at the next time under the consideration of the wheel-ground contact force comprises: In consideration of the wheel-ground contact force , the wheel-ground contact force at the current time is obtained , the predicted motion state at the next time is determined ; wherein the second predicted motion state at the next time instant The formula is: wherein a first predicted motion state for the next time instant is generated superimposing a disturbance of the current time instant's wheel-ground contact force to generate a second predicted motion state for the next time instant .

5. The method of claim 3, wherein, The step of obtaining the observation data of the two-wheeled vehicle system at the next time based on the preset observation function, and constructing the optimization objective function at the next time based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state and the observation data comprises: Based on a predetermined observation function , obtaining observation data of the two-wheeled vehicle system at a next time ; a first predicted motion state for a next time instant , a predicted covariance matrix , a second predicted motion state with the observation data , an optimization objective function is constructed wherein the formula of the optimization objective function is: wherein is the a posteriori motion state for the next time instant, is the wheel-ground contact force for the next time instant; is a first order partial derivative matrix of the observation function with respect to the motion state ; is a first order partial derivative matrix of the observation function with respect to the wheel-ground contact force ; is a correction of the a posteriori motion state with respect to the first predicted motion state; , is a predicted observation data in the case of neglecting the wheel-ground contact force ; The step of solving the optimization objective function at the next time under the premise of meeting the preset physical condition, determining the posterior motion state and wheel-ground contact force at the next time corresponding to the optimal solution of the optimization objective function comprises: solving the optimization objective function of the next time instant under the premise of meeting the preset physical condition, determining the posterior motion state of the next time instant corresponding to the optimal solution of the optimization objective function the wheel-ground contact force .

6. The method of claim 5, wherein, The preset physical condition required to be met by the optimization objective function comprises: contact occurrence condition and tangent force linear complementarity condition; The formula of the contact occurrence condition is: wherein, is the normal distance of the wheel to the ground, is the normal contact force experienced by the wheel at the current time instant; The formula of the tangent force linear complementarity condition is: in, Let be the basis vector matrix of the tangent plane. To constrain the Jacobian matrix, For generalized velocity vectors, For Lagrange multipliers, Let be the Coulomb friction coefficient. It is a vector of all 1s, with dimensions equal to 1. Consistent, This is the vector of tangential force coefficients. It is a complementary symbol.

7. The method of claim 6, wherein, The step of performing posterior updating on the predicted covariance matrix to determine the posterior covariance matrix at the next time comprises: constructing a Kalman gain , the Kalman gain is given by the formula wherein is the noise covariance matrix is the observation matrix is the transpose matrix of using the Kalman gain with the observation matrix updating the prediction covariance matrix to determine a posteriori covariance matrix of the two-wheeled vehicle system at the next time ; wherein the formula of the a posteriori covariance matrix of the two-wheeled vehicle system at the next time is: .

8. A two-wheeled vehicle contact force estimation device based on a first kind of Lagrange equation, characterized by, The device comprises: The data acquisition module is configured to acquire the a posteriori motion state, the control input and the a posteriori covariance matrix of the two-wheeled vehicle system at the current time point. The first prior propagation module is configured to determine the first predicted motion state and the predicted covariance matrix of the two-wheeled vehicle system at the next time point based on the preset discrete dynamic equation of the two-wheeled vehicle system, without considering the wheel-ground contact force. The second prior propagation module is configured to superimpose the disturbance of the wheel-ground contact force in the first predicted motion state at the next time point to generate the second predicted motion state of the two-wheeled vehicle system at the next time point, with the wheel-ground contact force being considered. The optimization function construction module is configured to acquire the observation data of the two-wheeled vehicle system at the next time point based on a preset observation function, and construct an optimization objective function at the next time point based on the first predicted motion state, the predicted covariance matrix, the second predicted motion state and the observation data, wherein the variables of the observation function and the optimization objective function both include the motion state and the wheel-ground contact force. The observation update module is configured to solve the optimization objective function at the next time point under the premise of satisfying a preset physical condition, determine the a posteriori motion state and the wheel-ground contact force at the next time point corresponding to the optimal solution of the optimization objective function, and perform a posteriori update on the predicted covariance matrix to determine the a posteriori covariance matrix at the next time point.

9. An electronic device, comprising: The memory is configured to store a computer program. The processor is configured to implement the steps of the two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation according to any one of claims 1 to 6 when the computer program is executed. The computer readable storage medium stores a computer program, and the computer program is executed by the processor to implement the steps of the two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation according to any one of claims 1 to 5.

10. A computer-readable storage medium, characterized in that, The computer program is executed by the processor to implement the steps of the two-wheeled vehicle contact force estimation method based on the first type of Lagrange equation according to any one of claims 1 to 5.

11. A computer program product comprising a computer program, characterized in that, ​

Images