Position and posture capturing method and device applied to motion capture system

Abstract

The application provides a position and posture capturing method and system applied to a motion capture system, and aims to solve the technical problem of low motion capture precision. The position and posture capturing method of the motion capture system comprises the following steps: obtaining a nominal position and posture transformation matrix and an actual position and posture transformation matrix through joint angles, end effector speed and end effector angular velocity; then, a position and posture deviation vector is calculated; the position and posture deviation vector is input into a least square matrix equation to obtain geometric error and random colored noise; based on an expectation maximization algorithm, the geometric error and the random colored noise are converged to obtain actual kinematic parameters; the actual kinematic parameter value is input into the motion capture system to obtain a calibrated motion capture system; the joint angles are input into the motion capture system to output a simulation posture and position, the error caused by the colored noise is reduced, and the calibration precision of the kinematic parameters is improved.

Description

Technical Field

[0001] This application relates to the field of motion capture technology, and in particular to a position and attitude capture method and system applied to motion capture systems. Background Technology

[0002] Motion capture (MoCap) technology is widely used in telemedicine, virtual reality, and sports training. Specifically, motion capture technology is a technique that uses cameras (such as RGB, infrared, or optical cameras), mechanical devices, and inertial sensors to acquire the position and orientation of the entire human body or a part of it. In practical applications, inertial motion capture (I-MoCap) technology is a relatively new type of motion capture technology. It uses wireless motion and posture sensors to first collect the posture and orientation of body parts, then uses the principles of human kinematics to reconstruct a human motion model, and simultaneously presents the data wirelessly in computer software. Inertial motion capture technology has been widely adopted due to its advantages such as portability, low cost, and insensitivity to ambient light conditions.

[0003] In inertial motion capture technology, kinematic parameter calibration is required to improve motion capture accuracy. Traditional calibration methods, such as the least squares method widely used in industrial arms, are employed to calibrate these parameters.

[0004] In implementing the existing technology, the inventors discovered that:

[0005] During data acquisition, random colored noise is generated due to factors such as gyroscope drift. Traditional calibration methods cannot effectively reduce the impact of random colored noise such as gyroscope drift. Therefore, the acquired data is a biased estimate, resulting in low motion capture accuracy.

[0006] Therefore, there is a need to provide a position and attitude capture method and system for motion capture systems to solve the technical problem of low motion capture accuracy. Summary of the Invention

[0007] This application provides a position and attitude capture method and system for use in motion capture systems to solve the technical problem of low motion capture accuracy.

[0008] Specifically, a position and pose capture method applied to a motion capture system includes:

[0009] Acquire joint angles, end effector speed, and end effector angular velocity in the motion capture system;

[0010] Input the joint angles into the initial kinematic model to obtain the nominal position and attitude transformation matrix;

[0011] Input the end effector velocity and end effector angular velocity into the lever effect equation to obtain the actual position and attitude transformation matrix;

[0012] The position and attitude deviation vector is calculated based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix.

[0013] Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise;

[0014] Based on the expectation-maximization algorithm, the geometric error and random colored noise are converged to obtain the actual kinematic parameters;

[0015] Input the actual kinematic parameter values ​​into the motion capture system to obtain the calibrated motion capture system;

[0016] Input the joint angle to the motion capture system, and output the simulated position and posture.

[0017] Furthermore, the input joint angles are transformed into the initial kinematic model to obtain the nominal position and pose transformation matrix, including:

[0018] Establish a three-dimensional coordinate system and an initial kinematic model located in the three-dimensional coordinate system; the initial kinematic model has at least an initial joint and an end joint;

[0019] Based on the initial kinematic model, a homogeneous transformation matrix of the distal joint relative to the proximal joint is generated as the initial kinematic parametric equation.

[0020] Input joint angles into the initial kinematic model to establish a mapping relationship between the motion capture system and the initial kinematic model;

[0021] Based on the joint angles, update the initial kinematic parameter equations to generate the nominal position and attitude transformation matrix.

[0022] Furthermore, the input end effector velocity, end effector angular velocity, and lever effect equation are used to obtain the actual position and attitude transformation matrix, including:

[0023] Based on the cross product operation, the end effector velocity and end effector angular velocity are input into the lever effect equation to obtain the end effector position;

[0024] The direction of the end effector is obtained through the gyroscope and accelerometer in the motion capture system;

[0025] Based on the end effector position and end effector orientation, the actual position and attitude transformation matrix is ​​obtained.

[0026] Furthermore, based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, the position and attitude deviation vector is calculated, including:

[0027] Based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, the homogeneous transformation matrix of the end joint relative to the starting joint is obtained.

[0028] Based on the homogeneous transformation matrix, determine the equation expression containing the position deviation vector, attitude deviation vector, and position and attitude deviation vector;

[0029] By aligning the first-order differential of the transformation matrix, we obtain the position deviation vector and the attitude deviation vector.

[0030] Substitute the position deviation vector and attitude deviation vector into the equation expression to calculate the position and attitude deviation vector.

[0031] Furthermore, the input position and attitude deviation vector is converted to a least-squares matrix equation to obtain geometric error and random colored noise, including:

[0032] Based on the least squares method, a least squares matrix equation is generated that includes position and attitude deviation vector, geometric error, and random colored noise.

[0033] Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise.

[0034] This application also provides a calibration device for kinematic parameters in a motion capture system, comprising:

[0035] The acquisition module is used to acquire joint angles, end effector speeds, and end effector angular velocities in the motion capture system.

[0036] The calculation module is used to input joint angles into the initial kinematic model to obtain the nominal position and attitude transformation matrix; it is also used to input the end effector velocity and end effector angular velocity into the lever effect equation to obtain the actual position and attitude transformation matrix; and it is also used to calculate the position and attitude deviation vector based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix.

[0037] The calibration module is used to input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise; it is also used to converge the geometric error and random colored noise based on the expectation-maximization algorithm to obtain the actual kinematic parameters.

[0038] The simulation module is used to input actual kinematic parameter values ​​into the motion capture system to obtain the calibrated motion capture system; it is also used to input joint angles into the motion capture system and output simulated position and posture.

[0039] Furthermore, the calculation module is used to input joint angles into the initial kinematic model to obtain the nominal position and attitude transformation matrix, specifically for:

[0040] Establish a three-dimensional coordinate system and an initial kinematic model located in the three-dimensional coordinate system; the initial kinematic model has at least an initial joint and an end joint;

[0041] Based on the initial kinematic model, a homogeneous transformation matrix of the distal joint relative to the proximal joint is generated as the initial kinematic parametric equation.

[0042] Input joint angles into the initial kinematic model to establish a mapping relationship between the motion capture system and the initial kinematic model;

[0043] Based on the joint angles, update the initial kinematic parameter equations to generate the nominal position and attitude transformation matrix.

[0044] Furthermore, the calculation module is also used to input the end effector velocity and angular velocity to the lever effect equation, and obtain the actual position and attitude transformation matrix, specifically for:

[0045] Based on the cross product operation, the end effector velocity and end effector angular velocity are input into the lever effect equation to obtain the end effector position;

[0046] The direction of the end effector is obtained through the gyroscope and accelerometer in the motion capture system;

[0047] Based on the end effector position and end effector orientation, the actual position and attitude transformation matrix is ​​obtained.

[0048] Furthermore, it is also used to calculate the position and attitude deviation vector based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, including:

[0049] Based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, the homogeneous transformation matrix of the end joint relative to the starting joint is obtained.

[0050] Based on the homogeneous transformation matrix, determine the equation expression containing the position deviation vector, attitude deviation vector, and position and attitude deviation vector;

[0051] By aligning the first-order differential of the transformation matrix, we obtain the position deviation vector and the attitude deviation vector.

[0052] Substitute the position deviation vector and attitude deviation vector into the equation expression to calculate the position and attitude deviation vector.

[0053] Furthermore, the calibration module is used to input the position and attitude deviation vector into a least-squares matrix equation to obtain the geometric error and random colored noise, specifically for:

[0054] Based on the least squares method, a least squares matrix equation is generated that includes position and attitude deviation vector, geometric error, and random colored noise.

[0055] Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise.

[0056] The technical solution provided in this application has at least the following beneficial effects:

[0057] By using the least squares method to calibrate geometric parameters, the number of iterations under the expectation-maximization framework is reduced, improving the efficiency of this method in embedded systems. During data acquisition, based on the expectation-maximization algorithm, geometric errors and random colored noise are converged to obtain actual kinematic parameters, effectively reducing the impact of random colored noise such as gyroscope drift and improving motion capture accuracy. Attached Figure Description

[0058] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:

[0059] Figure 1 The exoskeleton inertial motion capture system provided in the embodiments of this application;

[0060] Figure 2 A flowchart illustrating a position and attitude capture method for a motion capture system, as provided in an embodiment of this application;

[0061] Figure 3 This refers to the three-dimensional coordinate system corresponding to the motion capture system provided in the embodiments of this application;

[0062] Figure 4 This application provides a schematic diagram of the structure of a position and attitude capture device applied to a motion capture system.

[0063] Figure 5 A calibration result diagram based on the least squares method provided for an embodiment of this application;

[0064] Figure 6 The calibration results diagram provided in the embodiment of this application under the expectation maximization framework;

[0065] Figure 7 A graph of convergence functions under the expectation maximization framework provided in the embodiments of this application.

[0066] The reference numerals in the figure are as follows:

[0067] 100 - Position and attitude capture device used in motion capture systems

[0068] 11-Acquisition Module

[0069] 12-Calculation Module

[0070] 13-Calibration Module

[0071] 14-Simulation module. Detailed Implementation

[0072] To make the objectives, technical solutions, and advantages of this application clearer, the technical solutions of this application will be clearly and completely described below in conjunction with specific embodiments and corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0073] Please refer to Figure 1 This is the exoskeleton inertial motion capture system designed for this application.

[0074] To address the technical problem of low motion capture accuracy, this application provides a position and posture capture method for use in motion capture systems. The motion capture system is an exoskeleton inertial motion capture system designed based on a 7-DOF human upper limb kinematic model. Since the exoskeleton inertial motion capture system is primarily based on the configuration of the human upper limb, the model is portable and easy to wear.

[0075] The exoskeleton inertial motion capture system can be understood as a three-dimensional serial model. The exoskeleton inertial motion capture system includes three 9-DOF inertial measurement units (IMUs). The dynamic accuracy of each 9-DOF IMU is 2 degrees, and its static accuracy reaches 0.7 degrees. Each 9-DOF IMU includes a three-axis digital accelerometer, three gyroscopes, and three magnetometers.

[0076] The CPU (central processing unit) of the exoskeleton inertial motion capture system consists of a K60DN512 processor. The core is an ARM Cortex-M4, and the CPU frequency is 100MHz.

[0077] Please refer to Figure 2 The calibration method of the motion capture system specifically includes:

[0078] S100: Acquire joint angles, end effector speeds, and end effector angular velocities in the motion capture system.

[0079] Understandably, in one application scenario of this application, after wearing an exoskeleton inertial motion capture system and completing a series of random movements, the joint angles of each joint are obtained. These joint angles can be represented as θ. The joint angle θ can be obtained through the gyroscope, three-axis digital accelerometer, and magnetometer in the IMU. Additionally, the joint angular velocity can be obtained through the gyroscope output.

[0080] Acceleration is obtained using a triaxial digital accelerometer;

[0081] Integrating the acceleration, we obtain the velocity of the end effector.

[0082] The angular velocity of the end effector can be measured using a gyroscope.

[0083] The speed of the end effector can be expressed as v = [v x ,v y ,v z ] T The angular velocity of the end effector can be expressed as ω = [ω x ,ω y ,ω z ] T .

[0084] S200: Input joint angles to the initial kinematic model to obtain the nominal position and attitude transformation matrix.

[0085] The joint angle can be understood as the joint angle of each joint obtained after the motion capture system completes a series of random actions.

[0086] Furthermore, in a preferred embodiment provided in this application, the step of inputting joint angles to the initial kinematic model to obtain the nominal position and pose transformation matrix includes:

[0087] Establish a three-dimensional coordinate system and an initial kinematic model located in the three-dimensional coordinate system; the initial kinematic model has at least an initial joint and an end joint;

[0088] Based on the initial kinematic model, a homogeneous transformation matrix of the distal joint relative to the proximal joint is generated as the initial kinematic parametric equation.

[0089] Input joint angles into the initial kinematic model to establish a mapping relationship between the motion capture system and the initial kinematic model;

[0090] Based on the joint angles, update the initial kinematic parameter equations to generate the nominal position and attitude transformation matrix.

[0091] Understandably, the kinematic parameter calibration method proposed in this application is based on a 7-DOF human upper limb kinematic model, and an exoskeleton inertial motion capture system is designed accordingly. Please refer to... Figure 3 , which is the three-dimensional coordinate system established in this application.

[0092] Based on the exoskeleton inertial motion capture system, a three-dimensional coordinate system and an initial kinematic model located within this system are established. The three-dimensional coordinate system can be understood as a link coordinate system, comprising at least a base coordinate system and a final coordinate system. The link coordinate system can contain seven joints. Correspondingly, the initial kinematic model has at least initial joints and final joints. The base coordinate system is the initial joint coordinate system, and the final coordinate system is the final joint coordinate system.

[0093] The initial kinematic model should include at least the following parameters: joint angle, link offset, link length, and link torsion angle.

[0094] Joint angle θ i Indicates surrounding z i Axis from x i Rotate axis to x i+1 The angle required for the axis.

[0095] Linkage offset d i Indicates along z i Axis measurement x i axis and x i+1 The distance between axes.

[0096] Link length a i Indicates along x i+1 Axis measurement z i axis and z i+1 The distance between axes.

[0097] Linkage torsion angle α i Indicates around x i Axis from z i Rotate axis to z i+1 The required angle for the axis.

[0098] i represents the joint number (i = 1, 2, ..., 7).

[0099] Wherein, the length a of the connecting rod i , connecting rod torsion angle α i It is a constant.

[0100] Joint angle θ i For rotational joint variables;

[0101] Linkage offset d i For the movement joint variable.

[0102] The kinematic parameters of the exoskeleton inertial motion capture system are defined according to the Denavit-Hartenberg (DH) convention, as shown in Table 1.

[0103] Table 1 Standard DH Parameters for Exoskeleton Inertial Motion Capture Systems

[0104]

[0105] Based on the initial kinematic model, a homogeneous transformation matrix of the distal joint relative to the proximal joint is generated, which serves as the initial kinematic parametric equation.

[0106] Understandably, based on the parameters in the initial kinematic model, and according to the DH transformation, the homogeneous transformation from the (i-1)th joint to the ith joint can be expressed as:

[0107]

[0108] Where c represents cos(·) and s represents sin(·).

[0109] Based on the homogeneous transformation from the (i-1)th joint to the ith joint, generate the homogeneous transformation matrix of the distal joint relative to the originating joint:

[0110]

[0111] The homogeneous transformation matrix of the distal joint relative to the proximal joint serves as the initial kinematic parameter equation. Where [p x ,p y ,p z ,β x ,β y ,β z ] T This represents the position and orientation of the end effector in Cartesian space.

[0112] Furthermore, generating the homogeneous transformation matrix of the distal joint relative to the originating joint based on the homogeneous transformation from the (i-1)th joint to the ith joint includes:

[0113]

[0114] Therefore, the homogeneous transformation matrix of the distal joint relative to the proximal joint can be obtained:

[0115]

[0116] Input joint angles into the initial kinematic model to establish a mapping relationship between the motion capture system and the initial kinematic model. Update the initial kinematic parameter equations based on the joint angles to generate the nominal position and pose transformation matrix.

[0117] Substituting the joint angles into the homogeneous transformation matrix of the distal joint relative to the originating joint yields the nominal position and orientation transformation matrix. This updates the initial kinematic parameter equations in the initial kinematic model, improving nominal efficiency.

[0118] Furthermore, for a 7-DOF exoskeleton inertial motion capture system, the position and orientation of the end effector are analytically described as a nonlinear mapping. Specifically, by equalizing corresponding bits of the matrix and using the homogeneous transformation matrix of the end joint relative to the starting joint, we can obtain:

[0119] Nom=f(ζ,θ), (5)

[0120] Where Nom = [p x ,p y ,p z ,β x ,β y ,β z ] T It is also called the nominal model.

[0121] By making corresponding bits of the matrix equal, f(ζ,θ)=[p x ,p y ,p z ,β x ,β y ,β z ] T It can be further described as:

[0122] p x =A4

[0123] p y =A8

[0124] p z =A 12

[0125] β x =arcsin(A3 / sin(arccos(A 11 ))),

[0126] β y =arccos(A 11 )

[0127] β z =arcsin(A9 / sin(arccos(A 11 ))) (6)

[0128] Where A1, A2, ... A 12 It can be obtained through matrix multiplication.

[0129] It is evident that the exoskeleton inertial motion capture system is fixed to the human upper limb. The initial kinematic parameters satisfy basic biological characteristics. These biological characteristics are then transformed into nonlinear inequality constraints for the motion capture system.

[0130] Furthermore, the biometric features are transformed into nonlinear inequality constraints for the motion capture system, including:

[0131] Obtain the homogeneous transformation of the end effector relative to the base coordinate system;

[0132] Based on the rule of equality of corresponding positions in the matrix, establish the corresponding nonlinear mapping;

[0133] Obtain the range of geometric parameters according to the SAE J833 standard;

[0134] Based on anatomical principles, obtain the range of joint rotation angles;

[0135] Based on the range of geometric parameters and the range of joint rotation angles, nonlinear inequality constraints are obtained.

[0136] In one specific embodiment, the range of kinematic parameters is shown in Table 2.

[0137] Table 2 Range of geometric parameters for each joint

[0138]

[0139] As shown in Table 2, the range of geometric parameters is obtained according to the SAE J833 standard. Table 2 provides the maximum and minimum values ​​of the geometric parameters.

[0140] Based on anatomical principles, the range of joint rotation angles was obtained, as shown in Table 3.

[0141] Table 3 Joint Rotation Range

[0142]

[0143] As can be seen from Tables 2 and 3, the nonlinear inequality constraints are as follows:

[0144] ζ imin ≤ζ i ≤ζ imax ,

[0145] θ imin ≤θ i ≤θ imax ,

[0146] Where i = 1, 2, ..., 7.

[0147] S300: Input the end effector velocity and end effector angular velocity to the lever effect equation to obtain the actual position and attitude transformation matrix.

[0148] Understandably, the leverage effect equation can be explained as follows:

[0149] v=ω×r (7)

[0150] Where v = [v x ,v y ,v z ] T Indicates the speed of the end effector.

[0151] ω=[ω x ,ω y ,ω z ] T Represents the angular velocity of the end effector.

[0152] r represents the position of the end effector in the base coordinate system.

[0153] Furthermore, in a preferred embodiment provided in this application, the process of inputting the end effector velocity, end effector angular velocity to the lever effect equation to obtain the actual position and attitude transformation matrix includes:

[0154] Based on the cross product operation, the end effector velocity and end effector angular velocity are input into the lever effect equation to obtain the end effector position;

[0155] The direction of the end effector is obtained through the gyroscope and accelerometer in the motion capture system;

[0156] Based on the end effector position and end effector orientation, the actual position and attitude transformation matrix is ​​obtained.

[0157] In one specific embodiment of this application, according to the cross product operation, Equation 7 can be transformed into:

[0158]

[0159] It is worth noting that,

[0160]

[0161] In summary, the end effector position and orientation can be obtained. Then, based on the end effector position and orientation, the actual position and attitude transformation matrix is ​​derived.

[0162] Data is acquired through the inertial measurement unit (IMU) in the motion capture system, improving data accuracy. Furthermore, obtaining the actual position and attitude transformation matrix is ​​simple and requires no unnecessary complex calculations.

[0163] S400: Calculate the position and attitude deviation vector based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix.

[0164] The position and attitude deviation vector includes the position deviation vector and the attitude deviation vector.

[0165] Furthermore, in a preferred embodiment provided in this application, the position and attitude deviation vector is calculated based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, including:

[0166] Based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, the homogeneous transformation matrix of the end joint relative to the starting joint is obtained.

[0167] Based on the homogeneous transformation matrix, determine the equation expression containing the position deviation vector, attitude deviation vector, and position and attitude deviation vector;

[0168] By aligning the first-order differential of the transformation matrix, we obtain the position deviation vector and the attitude deviation vector.

[0169] Substitute the position deviation vector and attitude deviation vector into the equation expression to calculate the position and attitude deviation vector.

[0170] Specifically, based on the nominal position and orientation transformation matrix and the actual position and orientation transformation matrix, the homogeneous transformation matrix of the end joint relative to the starting joint is obtained.

[0171] Based on the homogeneous transformation matrix, determine the equation expression containing the position deviation vector, attitude deviation vector, and position and attitude deviation vector.

[0172] In a specific embodiment of this application, the first-order differential of the homogeneous transformation matrix of the distal joint relative to the originating joint is first expressed.

[0173]

[0174] in, Represents the actual transformation matrix, Represents the nominal transformation matrix. This represents the motion error matrix.

[0175] According to Equation 10, It can be represented as follows:

[0176]

[0177] Among them, [δ xi ,δ yi ,δ zi ] T It is the direction error vector, [l xi ,l yi ,l zi ] T It is the position error vector.

[0178] For motion capture systems, the kinematic parameters of the joints affect the accuracy of motion capture.

[0179] Therefore, in the Cartesian coordinate system, Represented as:

[0180]

[0181] According to (10) and (12), it can be transformed into:

[0182]

[0183] Combining (12) and (13), we can obtain:

[0184]

[0185] in,

[0186]

[0187] Combining (10) and (14), It can be represented as

[0188]

[0189] Therefore, the position and attitude deviation vector Δ T This can be represented as follows:

[0190]

[0191] in,

[0192] l i =[l xi ,l yi ,l zi ] T δ i =[δ xi ,δ yi ,δ zi ] T F1 = p i+1 ×f i+1 F2 = p i+1 ×o i+1 F3 = p i+1 ×c i+1 F4 = o i+1 ×c i+1 F5 = c i+1 ×f i+1 F6 = f i+1 ×o i+1 .

[0193] Specifically,

[0194] Equation (16) is derived from equation (15). The derivation process is as follows:

[0195]

[0196] Based on the fundamental principles of matrix inversion, we can obtain:

[0197]

[0198] Rotation matrices have the following properties

[0199] [f i+1 o i+1 c i+1 ] -1 =[f i+1 o i+1 c i+1 ] T (19)

[0200] By substituting (18) and (19) into (17), we can see that:

[0201]

[0202] Then, by aligning the corresponding positions in the matrix, the position and attitude deviation vector Δ is obtained. T Expression.

[0203] By aligning the first-order differential of the transformation matrix, we obtain the position deviation vector and the attitude deviation vector.

[0204] Specifically, the first-order differential of the homogeneous transformation matrix of the distal joint relative to the proximal joint is constructed from the differential part, and is approximated as follows:

[0205]

[0206] definition

[0207] Understandably, C ai The derivation process is as follows:

[0208] According to Equation 1, the homogeneous transformation from the (i-1)th joint to the ith joint can be expressed as follows: as follows

[0209]

[0210] therefore, It can be represented as

[0211]

[0212] Then, according to the definition achievable

[0213]

[0214] By transforming equation (24), we obtain

[0215]

[0216] It can be seen that in homogeneous transformations, Rot z (θ i Tran z (d i ), Tran x (a i ), Rot x (α i All of them are invertible matrices.

[0217] Through calculation, we obtain:

[0218]

[0219]

[0220] Substituting equation (26) into equation (25), we get:

[0221]

[0222] Similarly, we can derive C di C αi C θi .

[0223] Combining the homogeneous transformation from joint (i-1) to joint i (1), C ai C di C αi and C θi It can be represented as:

[0224]

[0225]

[0226] Substitute (28) into (27), It can be represented as

[0227]

[0228] And according to (10), we can obtain

[0229]

[0230] Through (28) and (30), It can be represented as

[0231]

[0232] Where, M1=sα i △di +a i cα i △θ i M2 = cα i △d i -a i sα i △θ i .

[0233] According to (31), the position error l can be seen. i and direction error δ i It can be represented as:

[0234]

[0235]

[0236] Define k i,1 =[1,0,0] T ,k i,2 =[0,sα] i ,cα i ] T ,k i,3 =[0,a i cα i ,-a i sα i ] T ,k i,4 =[1,0,0] T ,k i,5 =[0,sα] i ,cα i ] T ,

[0237] (32) and (33) can be simplified to:

[0238]

[0239] Substituting the position deviation vector and attitude deviation vector (34) into equation (16), the position and attitude deviation vector is calculated:

[0240]

[0241] By aligning the first-order derivative of the transformation matrix, the deviations of each joint and the end effector are obtained, resulting in position deviation vectors and attitude deviation vectors. These vectors reflect the relationship between the deviations of each joint and the end effector, and the kinematic parameter calibration efficiency is improved through clear formulaic calibration.

[0242] S500: Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise.

[0243] The geometric error can be understood as the deviation between the actual kinematic parameters and the nominal kinematic parameters. An actual system can be defined as a nominal system with a geometric error Δζ and random colored noise Δθ, expressed as follows:

[0244] Act=f(ξ+△ξ,θ+△θ) (36)

[0245] Among them, geometric error Including △a, △d and

[0246] Specifically, △a, △d, △α, and △θ are defined as follows:

[0247]

[0248]

[0249]

[0250]

[0251] Therefore, the position and orientation accuracy of a motion capture system, i.e., the position and attitude deviation vector, can also be expressed as:

[0252] △ T =||A c t-Nom||2

[0253] =||f(ζ+△ζ,θ+△θ)-f(ζ,θ)||2, (37)

[0254] Where △ T =[l x ,l y ,l z ,δ x ,δ y ,δ z ] T .

[0255] Furthermore, the input position and attitude deviation vector is converted to a least-squares matrix equation to obtain geometric error and random colored noise, including:

[0256] Based on the least squares method, a least squares matrix equation is generated that includes position and attitude deviation vector, geometric error, and random colored noise.

[0257] Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise.

[0258] In one embodiment of this application,

[0259] Based on the least squares method, a least squares matrix equation is generated that includes the position and attitude deviation vector, geometric error, and random colored noise:

[0260] [△ζ T ,△θ T ] T =(Φ T Φ) -1 Φ T △ T (38)

[0261] in

[0262] Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error Δζ and random colored noise Δθ.

[0263] S600: Based on the expectation-maximization algorithm, it converges to obtain the actual kinematic parameters by acknowledging geometric errors and random colored noise.

[0264] The expectation-maximization algorithm can be understood as a general method for finding the maximum likelihood solution of a probabilistic model with latent variables. In a probabilistic model, all observed variables are defined as Z, and all latent variables are defined as M.

[0265] The joint distribution p(Z,M|Θ) is controlled by a set of specified parameters Θ.

[0266] Our goal is to maximize:

[0267]

[0268] Understandably, directly optimizing p(Z|Θ) is difficult, but optimizing p(Z,M|Θ) using the maximum likelihood function is much simpler. Next, we introduce a distribution q(M) defined on the latent variables. For any choice q(M), the following decomposition holds:

[0269]

[0270] We defined

[0271] It is worth noting that, It is a function of the distribution q(M) and the parameter Θ.

[0272] As shown in (42), KL(q||p) is the KL divergence between q(M) and the posterior distribution p(M|Z,Θ).

[0273] The EM algorithm is a two-stage iterative optimization method for finding solutions to ML problems. We can define the EM algorithm using decomposition (40) and prove that it does indeed maximize the log-likelihood.

[0274] Define Θ old This represents the current value of the parameter vector. In the e-step, the lower bound is maximized by adjusting q(M). And keep Θ old Fixed. In the next M steps, the distribution q(M) remains constant while maximizing by adjusting Θ. We have seen that both the E-step and M-step of the EM algorithm increase the value of the log-likelihood function. The EM algorithm will converge when the log-likelihood function has reached its maximum value or the parameter remains constant.

[0275] The expectation-maximization algorithm introduces a free joint distribution. Also known as the prior distribution, it serves as an approximation of the parametric distribution Δθ. i Calculate the log-likelihood function:

[0276] The mean, variance, and mixing coefficients of the geometric error and random colored noise are used as initial values ​​and input into the log-likelihood function.

[0277] Alternate between the expectation step and the maximization step to increase the log-likelihood function.

[0278] Specifically, in the expectation step, we use the current value of the parameter to evaluate the posterior probability, which is as follows:

[0279]

[0280] Then, in the maximization step, the posterior probability is used to re-estimate the mean, variance, and mixing coefficients.

[0281]

[0282]

[0283]

[0284] in and

[0285] In practice, the expectation-maximization algorithm is considered to have converged when the change in the log-likelihood function or parameters is below a set threshold.

[0286] In one specific embodiment of this application, the identification result of the least squares method is used as the initial value.

[0287] The objective function to be optimized is:

[0288]

[0289] Where K represents the total number of data points (each data point is represented by k).

[0290] In (5), ζ is represented by ζ′=ζ+△ζ and θ′=θ+△θ is represented by θ. T It will be reduced. Then, the distribution Δθ i It is defined as a Gaussian distribution (i.e., The likelihood function is calculated based on the maximum likelihood estimate. The likelihood function is as follows:

[0291]

[0292] Where π i This represents a mixed parameter, and it must satisfy 0 ≤ π. i ≤1.

[0293] z k Represents the k-th error vector Δ T The first row of data.

[0294] μ k,i Represents △θ i Expectation.k k,i It is a constant value obtained from J2.

[0295] Σ k,i Represents △θ i The variance.

[0296] According to (48), we can see that it no longer has a closed-form analytical solution because there is a summation with respect to i in the logarithm. Therefore, the parameters cannot be estimated by differentiating the likelihood function. However, we can use the EM algorithm to obtain a numerical solution.

[0297] Finally, by using the mean of the Gaussian distribution in the EM algorithm as the parameter estimate, the influence of the mixed calibration problem in the maximum likelihood method can be reduced, thus reducing the number of iterations in the expectation maximization algorithm.

[0298] S700: Input the actual kinematic parameter values ​​into the motion capture system to obtain the calibrated motion capture system;

[0299] S800: Input joint angles to the motion capture system and output simulated position and posture.

[0300] Furthermore, the inventors found through simulation that the capture accuracy based on the expectation-maximization algorithm was significantly improved by 16.79% compared to the single least squares method.

[0301] The simulation process is as follows:

[0302] Set the kinematic parameters in MATLAB. Table 4 shows the assumed DH parameters for the motion capture system.

[0303] Table 4 Assumptions for DH parameters in the motion capture system

[0304]

[0305] In Table 4, Δθ(k) is a first-order Gaussian-Markov process.

[0306] The relevant parameters are defined as follows: τ = 1s, and σ = 1.

[0307] Based on the least squares matrix equation that includes position and attitude deviation vectors, geometric errors, and random colored noise, a random trajectory was used to obtain the calibration results, as shown in Table 5.

[0308] Table 5. Calibration results using the single least squares method.

[0309]

[0310]

[0311] The motion trajectories of the actual system, the nominal system, and the nominal system based on the calibration results were plotted respectively, as follows: Figure 3 As shown, the calibration effect was verified.

[0312] The experimental results are as follows:

[0313] from Figure 3 As can be seen, the calibrated trajectory is closer to the actual trajectory. The difference between the target value y and the initial trajectory is 7.8153, and the difference between the actual trajectory and the calibrated trajectory is 0.6211. When using the LS method, the target value is reduced by 92.05%. Therefore, the geometric error calibration model can be effectively applied to motion capture systems to improve motion capture accuracy.

[0314] Furthermore, the inventors found through semi-physical experiments that the capture accuracy based on the expectation-maximization algorithm was significantly improved by 7.16% compared to the single least squares method.

[0315] The experimental procedure is as follows:

[0316] Step 1: Wear an exoskeleton inertial motion capture system to complete a series of random movements.

[0317] Step 2: Calculate the joint angular velocity based on the gyroscope output.

[0318] Step 3: Obtain the joint angle θ based on the gyroscope, accelerometer, and magnetometer in the IMU.

[0319] Step 4: Substitute the joint angle θ into the initial DH parameters to obtain the end-effector nominal transformation matrix.

[0320] Step 5: Obtain the true transformation matrix at the end point based on the lever arm effect.

[0321] Step 6: Calculate the end-effector position and attitude deviation vector Δ T according to and

[0322] Step 7: By using △ T Substituting into (28), we can obtain △ζ and △θ.

[0323] Step 8: Identify random colored noise Δθ by introducing the EM algorithm.

[0324] The experimental results are as follows:

[0325] Table 6. Calibration results under least squares method

[0326]

[0327]

[0328] The calibration results under the least squares method shown in Table 6 are set as the initial values ​​for the expectation-maximization algorithm. The calibration results under the EM framework are shown in Table 7.

[0329] Table 7. Kinematic parameters calibrated under the expectation-maximization algorithm.

[0330] parameter <![CDATA[△θ1]]> <![CDATA[△θ2]]> <![CDATA[△θ3]]> <![CDATA[△θ4]]> <![CDATA[△θ5]]> <![CDATA[△θ6]]> <![CDATA[△θ7]]> average value -0.0877 -0.0219 -0.8644 0.5719 -0.1644 -0.1369 -0.2155 variance 0.0004 0.0005 0.0261 0.0114 0.0001 0.0015 0.0006 Posterior probability 0.0321 0.0134 0.0000 0.0000 0.5366 0.0001 0.4179

[0331] like Figure 4 To maximize the calibration effect within the framework.

[0332] Under the LS method, the target value y of the actual trajectory differs from that of the nominal trajectory by 0.3507.

[0333] Under the EM algorithm, the target value y of the actual trajectory and the nominal trajectory is 0.3256.

[0334] Figure 4 In this context, LS method calibration result represents the calibration result based on the least squares method.

[0335] Actual trajectory represents the actual trajectory; EM algorithm calibration result represents the calibration result based on the expectation-maximization algorithm.

[0336] like Figure 5For convergence under the EM framework, the capture accuracy was significantly improved by 7.16%.

[0337] Therefore, the convergence of the solution can be verified in a semi-physical experiment. Thus, this method is more suitable for embedded systems than the LS method.

[0338] In summary, this application proposes a position and attitude capture method for motion capture systems. First, it utilizes the least squares method to calculate the initial calibration results, reducing the number of iterations in the EM algorithm. Second, it employs the EM algorithm to correct parameters under the influence of random colored noise. Compared to the single LS method, this method improves the kinematic calibration performance, as the LS method is a biased estimate under random colored noise conditions. Simulation and experimental results show that this method improves motion capture accuracy by 16.79% and 7.16%, respectively, compared to the LS method.

[0339] This application also provides a multi-terminal control system for executing steps S100-S600.

[0340] Please refer to Figure 4 This application provides a multi-terminal data synchronization system 100 for multimedia teaching, specifically used for:

[0341] The acquisition module 11 is used to acquire joint angles, end effector speeds, and end effector angular velocities in the motion capture system.

[0342] It is understood that the kinematic parameter calibration method proposed in this application is based on a 7-DOF human upper limb kinematic model, and an exoskeleton inertial motion capture system is designed accordingly. Since the exoskeleton inertial motion capture system is primarily based on the configuration of the human upper limb, the model is portable and easy to wear. The exoskeleton inertial motion capture system can be understood as a three-dimensional serial model.

[0343] The exoskeleton inertial motion capture system includes three 9-DOF inertial measurement units (IMUs). Each IMU has a dynamic accuracy of 2 degrees and a static accuracy of 0.7 degrees. Each IMU comprises a three-axis digital accelerometer, three gyroscopes, and three magnetometers.

[0344] The CPU (central processing unit) of the exoskeleton inertial motion capture system consists of a K60DN512 processor. The core is an ARM Cortex-M4, and the CPU frequency is 100MHz.

[0345] In one application scenario of this application, after wearing an exoskeleton inertial motion capture system and completing a series of random movements, the joint angles of each joint are obtained. These joint angles can be represented as θ. The joint angle θ can be obtained through the gyroscope, three-axis digital accelerometer, and magnetometer in the IMU. Additionally, the joint angular velocity can be obtained through the gyroscope output.

[0346] Acceleration is obtained using a triaxial digital accelerometer;

[0347] Integrating the acceleration, we obtain the velocity of the end effector.

[0348] The angular velocity of the end effector can be measured using a gyroscope.

[0349] The speed of the end effector can be expressed as v = [v x ,v y ,v z ] T The angular velocity of the end effector can be expressed as ω = [ω x ,ω y ,ω z ] T .

[0350] The calculation module 12 is used to input joint angles into the initial kinematic model and obtain the nominal position and attitude transformation matrix.

[0351] The joint angle can be understood as the joint angle of each joint obtained after the motion capture system completes a series of random actions.

[0352] Furthermore, in a preferred embodiment provided in this application, the calculation module 12 is used to input joint angles to the initial kinematic model to obtain the nominal position and attitude transformation matrix, specifically for:

[0353] Establish a three-dimensional coordinate system and an initial kinematic model located in the three-dimensional coordinate system; the initial kinematic model has at least an initial joint and an end joint;

[0354] Based on the initial kinematic model, a homogeneous transformation matrix of the distal joint relative to the proximal joint is generated as the initial kinematic parametric equation.

[0355] Input joint angles into the initial kinematic model to establish a mapping relationship between the motion capture system and the initial kinematic model;

[0356] Based on the joint angles, update the initial kinematic parameter equations to generate the nominal position and attitude transformation matrix.

[0357] It is understood that the kinematic parameter calibration method proposed in this application is based on a 7-DOF human upper limb kinematic model, and an exoskeleton inertial motion capture system is designed. Based on the exoskeleton inertial motion capture system, a three-dimensional coordinate system and an initial kinematic model located within this three-dimensional coordinate system are established. This three-dimensional coordinate system can be understood as a link coordinate system, which includes at least a base coordinate system and a distal coordinate system. The link coordinate system can include 7 joints. Correspondingly, the initial kinematic model has at least an initial joint and a distal joint. The base coordinate system is the initial joint coordinate system, and the distal coordinate system is the distal joint coordinate system.

[0358] The initial kinematic model should include at least the following parameters: joint angle, link offset, link length, and link torsion angle.

[0359] Joint angle θ i Indicates surrounding z i Axis from x i Rotate axis to x i+1 The angle required for the axis.

[0360] Linkage offset d i Indicates along z i Axis measurement x i axis and x i+1 The distance between axes.

[0361] Link length a i Indicates along x i+1 Axis measurement z i axis and z i+1 The distance between axes.

[0362] Linkage torsion angle α i Indicates around x i Axis from z i Rotate axis to z i+1 The required angle for the axis.

[0363] i represents the joint number (i = 1, 2, ..., 7).

[0364] Wherein, the length a of the connecting rod i , connecting rod torsion angle α i It is a constant.

[0365] Joint angle θ i For rotational joint variables;

[0366] Linkage offset d i For the movement joint variable.

[0367] The kinematic parameters of the exoskeleton inertial motion capture system are defined according to the Denavit-Hartenberg (DH) convention, as shown in Table 8.

[0368] Table 8 Standard DH Parameters for Exoskeleton Inertial Motion Capture Systems

[0369]

[0370] Based on the initial kinematic model, a homogeneous transformation matrix of the distal joint relative to the proximal joint is generated, which serves as the initial kinematic parametric equation.

[0371] Understandably, based on the parameters in the initial kinematic model, and according to the DH transformation, the homogeneous transformation from the (i-1)th joint to the ith joint can be expressed as:

[0372]

[0373] Where c represents cos(·) and s represents sin(·).

[0374] Based on the homogeneous transformation from the (i-1)th joint to the ith joint, generate the homogeneous transformation matrix of the distal joint relative to the originating joint:

[0375]

[0376] The homogeneous transformation matrix of the distal joint relative to the proximal joint serves as the initial kinematic parameter equation. Where [p x ,p y ,p z ,β x ,β y ,β z ] T This represents the position and orientation of the end effector in Cartesian space.

[0377] Furthermore, generating the homogeneous transformation matrix of the distal joint relative to the originating joint based on the homogeneous transformation from the (i-1)th joint to the ith joint includes:

[0378]

[0379] Therefore, the homogeneous transformation matrix of the distal joint relative to the proximal joint can be obtained:

[0380]

[0381] Input joint angles into the initial kinematic model to establish a mapping relationship between the motion capture system and the initial kinematic model. Update the initial kinematic parameter equations based on the joint angles to generate the nominal position and pose transformation matrix.

[0382] Substituting the joint angles into the homogeneous transformation matrix of the distal joint relative to the originating joint yields the nominal position and orientation transformation matrix. This updates the initial kinematic parameter equations in the initial kinematic model, improving nominal efficiency.

[0383] Furthermore, for a 7-DOF exoskeleton inertial motion capture system, the position and orientation of the end effector are analytically described as a nonlinear mapping. Specifically, by equalizing corresponding bits of the matrix and using the homogeneous transformation matrix of the end joint relative to the starting joint, we can obtain:

[0384] Nom=f(ζ,θ), (5)

[0385] Where Nom = [p x ,p y ,p z ,β x ,β y ,β z ] T It is also called the nominal model.

[0386] By making corresponding bits of the matrix equal, f(ζ,θ)=[p x ,p y ,p z ,β x ,β y ,β z ] T It can be further described as:

[0387] p x =A4

[0388] p y =A8

[0389] p z =A 12

[0390] β x =arcsin(A3 / sin(arccos(A 11 ))),

[0391] β y =arccos(A 11 )

[0392] β z =arcsin(A9 / sin(arccos(A 11 ))) (6)

[0393] Where A1, A2, ... A 12 It can be obtained through matrix multiplication.

[0394] It is evident that the exoskeleton inertial motion capture system is fixed to the human upper limb. The initial kinematic parameters satisfy basic biological characteristics. These biological characteristics are then transformed into nonlinear inequality constraints for the motion capture system.

[0395] Furthermore, the biometric features are transformed into nonlinear inequality constraints for the motion capture system, including:

[0396] Obtain the homogeneous transformation of the end effector relative to the base coordinate system;

[0397] Based on the rule of equality of corresponding positions in the matrix, establish the corresponding nonlinear mapping;

[0398] Obtain the range of geometric parameters according to the SAE J833 standard;

[0399] Based on anatomical principles, obtain the range of joint rotation angles;

[0400] Based on the range of geometric parameters and the range of joint rotation angles, nonlinear inequality constraints are obtained.

[0401] In one specific embodiment, the range of kinematic parameters is shown in Table 9.

[0402] Table 9 Range of geometric parameters for each joint

[0403]

[0404] As shown in Table 9, the range of geometric parameters is obtained according to the SAE J833 standard. Table 9 provides the maximum and minimum values ​​of the geometric parameters.

[0405] Based on anatomical principles, the range of joint rotation angles was obtained, as shown in Table 10.

[0406] Table 10 Joint Rotation Range

[0407]

[0408] As can be seen from Tables 9 and 10, the nonlinear inequality constraints are as follows:

[0409] ζ imin ≤ζ i ≤ζ imax ,

[0410] θ imin ≤θ i ≤θ imax ,

[0411] Where i = 1, 2, ..., 7.

[0412] The calculation module 12 is also used to input the end effector velocity, end effector angular velocity to the lever effect equation, and obtain the actual position attitude transformation matrix.

[0413] Understandably, the leverage effect equation can be explained as follows:

[0414] v=ω×r (7)

[0415] Where v = [v x ,v y ,v z ] T Indicates the speed of the end effector.

[0416] ω=[ω x ,ω y ,ω z ] T Represents the angular velocity of the end effector.

[0417] r represents the position of the end effector in the base coordinate system.

[0418] Furthermore, in a preferred embodiment provided in this application, the calculation module 12 is also used to input the end effector velocity, end effector angular velocity to the lever effect equation, and obtain the actual position and attitude transformation matrix, specifically for:

[0419] Based on the cross product operation, the end effector velocity and end effector angular velocity are input into the lever effect equation to obtain the end effector position;

[0420] The direction of the end effector is obtained through the gyroscope and accelerometer in the motion capture system;

[0421] Based on the end effector position and end effector orientation, the actual position and attitude transformation matrix is ​​obtained.

[0422] In one specific embodiment of this application, according to the cross product operation, Equation 7 can be transformed into:

[0423]

[0424] It is worth noting that,

[0425]

[0426] In summary, the end effector position and orientation can be obtained. Then, based on the end effector position and orientation, the actual position and attitude transformation matrix is ​​derived.

[0427] Data is acquired through the inertial measurement unit (IMU) in the motion capture system, improving data accuracy. Furthermore, obtaining the actual position and attitude transformation matrix is ​​simple and requires no unnecessary complex calculations.

[0428] The calculation module 12 is also used to calculate the position and attitude deviation vector based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix.

[0429] The position and attitude deviation vector includes the position deviation vector and the attitude deviation vector.

[0430] Furthermore, in a preferred embodiment provided in this application, the calculation module 12 is further configured to calculate the position and attitude deviation vector based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, specifically for:

[0431] Based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, the homogeneous transformation matrix of the end joint relative to the starting joint is obtained.

[0432] Based on the homogeneous transformation matrix, determine the equation expression containing the position deviation vector, attitude deviation vector, and position and attitude deviation vector;

[0433] By aligning the first-order differential of the transformation matrix, we obtain the position deviation vector and the attitude deviation vector.

[0434] Substitute the position deviation vector and attitude deviation vector into the equation expression to calculate the position and attitude deviation vector.

[0435] Specifically, based on the nominal position and orientation transformation matrix and the actual position and orientation transformation matrix, the homogeneous transformation matrix of the end joint relative to the starting joint is obtained.

[0436] Based on the homogeneous transformation matrix, determine the equation expression containing the position deviation vector, attitude deviation vector, and position and attitude deviation vector.

[0437] In a specific embodiment of this application, the first-order differential of the homogeneous transformation matrix of the distal joint relative to the originating joint is first expressed.

[0438]

[0439] in, Represents the actual transformation matrix, Represents the nominal transformation matrix. This represents the motion error matrix.

[0440] According to Equation 10, It can be represented as follows:

[0441]

[0442] Among them, [δ xi ,δ yi ,δ zi ] T It is the direction error vector, [l xi ,l yi ,l zi ] T It is the position error vector.

[0443] For motion capture systems, the kinematic parameters of the joints affect the accuracy of motion capture.

[0444] Therefore, in the Cartesian coordinate system, Represented as:

[0445]

[0446] According to (10) and (12), it can be transformed into:

[0447]

[0448] Combining (12) and (13), we can obtain:

[0449]

[0450] in,

[0451]

[0452] Combining (10) and (14), It can be represented as

[0453]

[0454] Therefore, the position and attitude deviation vector Δ T This can be represented as follows:

[0455]

[0456] in,

[0457] l i =[l xi ,l yi ,l zi ] T δ i =[δ xi ,δ yi ,δ zi ] T F1 = p i+1 ×f i+1 F2 = p i+1 ×o i+1 F3 = p i+1 ×c i+1 F4 = o i+1 ×c i+1 F5 = c i+1 ×f i+1 F6 = f i+1 ×o i+1 .

[0458] Specifically,

[0459] Equation (16) is derived from equation (15). The derivation process is as follows:

[0460]

[0461] Based on the fundamental principles of matrix inversion, we can obtain:

[0462]

[0463] Rotation matrices have the following properties

[0464] [f i+1 o i+1 c i+1 ] -1 =[f i+1 o i+1 c i+1 ] T (19)

[0465] By substituting (18) and (19) into (17), we can see that:

[0466]

[0467] Then, by aligning the corresponding positions in the matrix, the position and attitude deviation vector Δ is obtained. T Expression.

[0468] By aligning the first-order differential of the transformation matrix, we obtain the position deviation vector and the attitude deviation vector.

[0469] Specifically, the first-order differential of the homogeneous transformation matrix of the distal joint relative to the proximal joint is constructed from the differential part, and is approximated as follows:

[0470]

[0471] definition

[0472] Understandably, C ai The derivation process is as follows:

[0473] According to Equation 1, the homogeneous transformation from the (i-1)th joint to the ith joint can be expressed as follows: as follows

[0474]

[0475] therefore, It can be represented as

[0476]

[0477] Then, according to the definition achievable

[0478]

[0479] By transforming equation (24), we obtain

[0480]

[0481] It can be seen that in homogeneous transformations, Rot z (θ i Tran z (d i ), Tran x (a i ), Rot x (α i All of them are invertible matrices.

[0482] Through calculation, we obtain:

[0483]

[0484]

[0485] Substituting equation (26) into equation (25), we get:

[0486]

[0487] Similarly, we can derive C di C αi C θi .

[0488] Combining the homogeneous transformation from joint (i-1) to joint i (1), C ai C di C αi and C θi It can be represented as:

[0489]

[0490]

[0491]

[0492] Substitute (28) into (27), It can be represented as

[0493]

[0494] And according to (10), we can obtain

[0495]

[0496] Through (28) and (30), It can be represented as

[0497]

[0498] Where, M1=sα i △d i +a i cα i △θ i M2 = cα i △d i -a i sα i △θ i .

[0499] According to (31), the position error l can be seen. i and direction error δ i It can be represented as:

[0500]

[0501]

[0502] Define k i,1 =[1,0,0] T ,k i,2 =[0,sα] i ,cα i ] T ,k i,3 =[0,a i cα i ,-a i sα i ] T ,k i,4 =[1,0,0] T ,k i,5 =[0,sα] i ,cα i ] T ,

[0503] (32) and (33) can be simplified to:

[0504]

[0505] Substituting the position deviation vector and attitude deviation vector (34) into equation (16), the position and attitude deviation vector is calculated:

[0506]

[0507] By aligning the first-order derivative of the transformation matrix, the deviations of each joint and the end effector are obtained, resulting in position deviation vectors and attitude deviation vectors. These vectors reflect the relationship between the deviations of each joint and the end effector, and the kinematic parameter calibration efficiency is improved through clear formulaic calibration.

[0508] The calibration module 13 is used to input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise.

[0509] The geometric error can be understood as the deviation between the actual kinematic parameters and the nominal kinematic parameters. An actual system can be defined as a nominal system with a geometric error Δζ and random colored noise Δθ, expressed as follows:

[0510] Act=f(ξ+△ξ,θ+△θ) (36)

[0511] Among them, geometric error Including △a, △d and

[0512] Specifically, △a, △d, △α, and △θ are defined as follows:

[0513]

[0514]

[0515]

[0516]

[0517] Therefore, the position and orientation accuracy of a motion capture system, i.e., the position and attitude deviation vector, can also be expressed as:

[0518] △ T =||Act-Nom||2

[0519] =||f(ζ+△ζ,θ+△θ)-f(ζ,θ)||2, (37)

[0520] Where △ T =[l x ,l y ,l z ,δ x ,δ y ,δ z ] T .

[0521] Furthermore, the input position and attitude deviation vector is converted to a least-squares matrix equation to obtain geometric error and random colored noise, including:

[0522] Based on the least squares method, a least squares matrix equation is generated that includes position and attitude deviation vector, geometric error, and random colored noise.

[0523] Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise.

[0524] In one embodiment of this application,

[0525] Based on the least squares method, a least squares matrix equation is generated that includes the position and attitude deviation vector, geometric error, and random colored noise:

[0526] [△ζ T ,△θ T ] T =(Φ T Φ) -1 Φ T △ T (38)

[0527] in

[0528] Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error Δζ and random colored noise Δθ.

[0529] The calibration module 13 is also used to converge geometric errors and random colored noise based on the expectation-maximization algorithm to obtain the actual kinematic parameters.

[0530] The expectation-maximization algorithm can be understood as a general method for finding the maximum likelihood solution of a probabilistic model with latent variables. In a probabilistic model, all observed variables are defined as Z, and all latent variables are defined as M.

[0531] The joint distribution p(Z,M|Θ) is controlled by a set of specified parameters Θ.

[0532] Our goal is to maximize:

[0533]

[0534] Understandably, directly optimizing p(Z|Θ) is difficult, but optimizing p(Z,M|Θ) using the maximum likelihood function is much simpler. Next, we introduce a distribution q(M) defined on the latent variables. For any choice q(M), the following decomposition holds:

[0535]

[0536] We defined

[0537]

[0538] It is worth noting that, It is a function of the distribution q(M) and the parameter Θ.

[0539] From (42), we can see that KL(q||p) is the KL divergence between q(M) and the posterior distribution p(M|Z,Θ).

[0540] The EM algorithm is a two-stage iterative optimization method for finding solutions to ML problems. We can define the EM algorithm using decomposition (40) and prove that it does indeed maximize the log-likelihood.

[0541] Define Θ old This represents the current value of the parameter vector. In the e-step, the lower bound is maximized by adjusting q(M). And keep Θ old Fixed. In the next M steps, the distribution q(M) remains constant while maximizing is achieved by adjusting Θ. We have seen that both the E-step and M-step of the EM algorithm increase the value of the log-likelihood function. The EM algorithm will converge when the log-likelihood function has reached its maximum value or the parameter remains constant.

[0542] The expectation-maximization algorithm introduces a free joint distribution. Also known as the prior distribution, it serves as an approximation of the parametric distribution Δθ. i Calculate the log-likelihood function:

[0543] The mean, variance, and mixing coefficients of the geometric error and random colored noise are used as initial values ​​and input into the log-likelihood function.

[0544] Alternate between the expectation step and the maximization step to increase the log-likelihood function.

[0545] Specifically, in the expectation step, we use the current value of the parameter to evaluate the posterior probability, which is as follows:

[0546]

[0547] Then, in the maximization step, the posterior probability is used to re-estimate the mean, variance, and mixing coefficients.

[0548]

[0549]

[0550]

[0551] in and

[0552] In practice, the expectation-maximization algorithm is considered to have converged when the change in the log-likelihood function or parameters is below a set threshold.

[0553] In one specific embodiment of this application, the identification result of the least squares method is used as the initial value.

[0554] The objective function to be optimized is:

[0555]

[0556] Where K represents the total number of data points (each data point is represented by k).

[0557] In (5), ζ is represented by ζ′=ζ+△ζ and θ′=θ+△θ is represented by θ. T It will be reduced. Then, the distribution Δθ i It is defined as a Gaussian distribution (i.e., The likelihood function is calculated based on the maximum likelihood estimate. The likelihood function is as follows:

[0558]

[0559] Where π i This represents a mixed parameter, and it must satisfy 0 ≤ π. i ≤1.

[0560] z k Represents the k-th error vector Δ T The first row of data.

[0561] μ k,i Represents △θ i Expectation.k k,i It is a constant value obtained from J2.

[0562] Σ k,i Represents △θ i The variance.

[0563] According to (48), we can see that it no longer has a closed-form analytical solution because there is a summation with respect to i in the logarithm. Therefore, the parameters cannot be estimated by differentiating the likelihood function. However, we can use the EM algorithm to obtain a numerical solution.

[0564] Finally, by using the mean of the Gaussian distribution in the EM algorithm as the parameter estimate, the influence of the mixed calibration problem in the maximum likelihood method can be reduced, thus reducing the number of iterations in the expectation maximization algorithm.

[0565] The simulation module 14 is used to input actual kinematic parameter values ​​into the motion capture system to obtain the calibrated motion capture system; it is also used to input joint angles into the motion capture system and output simulated position and posture.

[0566] Furthermore, simulations showed that the capture accuracy based on the expectation-maximization algorithm was significantly improved by 16.79% compared to the single least squares method. The convergence of the solution was verified in semi-physical experiments. Therefore, this method is more suitable for embedded systems than the LS method.

[0567] In summary, this application proposes a position and attitude capture device for motion capture systems. First, it utilizes the least squares method to calculate the initial calibration results, reducing the number of iterations in the EM algorithm. Second, it employs the EM algorithm to correct parameters under the influence of random colored noise. Compared to the single LS method, this method improves the kinematic calibration performance, as the LS method is a biased estimate under random colored noise conditions. Simulation and experimental results show that this method improves motion capture accuracy by 16.79% and 7.16% respectively compared to the LS method.

[0568] It should be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0569] The above description is merely an embodiment of this application and is not intended to limit the scope of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of the claims of this application.

Claims

1. A position and pose capture method applied to a motion capture system, characterized in that, include: Acquire joint angles, end effector speed, and end effector angular velocity in the motion capture system; A three-dimensional coordinate system is established, and an initial kinematic model is located in the three-dimensional coordinate system. The initial kinematic model has at least an initial joint and an end joint. Based on the initial kinematic model, a homogeneous transformation matrix of the distal joint relative to the proximal joint is generated as the initial kinematic parametric equation. Input joint angles into the initial kinematic model to establish a mapping relationship between the motion capture system and the initial kinematic model; Based on the joint angles, update the initial kinematic parameter equations and generate the nominal position and attitude transformation matrix; Input the end effector velocity and end effector angular velocity into the lever effect equation to obtain the actual position and attitude transformation matrix; The position and attitude deviation vector is calculated based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix. Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise; Based on the expectation-maximization algorithm, the geometric error and random colored noise are converged to obtain the actual kinematic parameters; Input the actual kinematic parameter values ​​into the motion capture system to obtain the calibrated motion capture system; Input the joint angle to the motion capture system, and output the simulated position and posture.

2. The method of claim 1, wherein, The input end effector velocity and end effector angular velocity are converted to the lever effect equation to obtain the actual position and attitude transformation matrix, including: Based on the cross product operation, the end effector velocity and end effector angular velocity are input into the lever effect equation to obtain the end effector position; The direction of the end effector is determined by a motion capture system; Based on the end effector position and end effector orientation, the actual position and attitude transformation matrix is ​​obtained.

3. The method of claim 1, wherein, Based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, the position and attitude deviation vector is calculated, including: Based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, the homogeneous transformation matrix of the end joint relative to the starting joint is obtained. Based on the homogeneous transformation matrix, determine the equation expression containing the position deviation vector, attitude deviation vector, and position and attitude deviation vector; By aligning the first-order differential of the transformation matrix, we obtain the position deviation vector and the attitude deviation vector. Substitute the position deviation vector and attitude deviation vector into the equation expression to calculate the position and attitude deviation vector.

4. The method of claim 1, wherein, The input position and attitude deviation vector is converted to a least-squares matrix equation to obtain geometric error and random colored noise, including: Based on the least squares method, a least squares matrix equation is generated that includes position and attitude deviation vector, geometric error, and random colored noise. Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise.

5. A position and attitude capture device applied to a motion capture system, characterized in that, include: The acquisition module is used to acquire joint angles, end effector speeds, and end effector angular velocities in the motion capture system. The calculation module is used to establish a three-dimensional coordinate system and an initial kinematic model located in the three-dimensional coordinate system. The initial kinematic model has at least an initial joint and an end joint. Based on the initial kinematic model, it generates a homogeneous transformation matrix of the end joint relative to the initial joint, which serves as the initial kinematic parameter equation. It inputs joint angles to the initial kinematic model, establishes a mapping relationship between the motion capture system and the initial kinematic model, and updates the initial kinematic parameter equation based on the joint angles to generate a nominal position and attitude transformation matrix. It is also used to input the end effector velocity and end effector angular velocity to the lever effect equation to obtain the actual position and attitude transformation matrix. It is also used to calculate the position and attitude deviation vector based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix; The calibration module is used to input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise; it is also used to converge the geometric error and random colored noise based on the expectation-maximization algorithm to obtain the actual kinematic parameters. The simulation module is used to input actual kinematic parameter values ​​into the motion capture system to obtain the calibrated motion capture system; it is also used to input joint angles into the motion capture system and output simulated position and posture.

6. The apparatus of claim 5 wherein, The calculation module is also used to input the end effector velocity and angular velocity to the lever effect equation, and obtain the actual position and attitude transformation matrix, specifically for: Based on the cross product operation, the end effector velocity and end effector angular velocity are input into the lever effect equation to obtain the end effector position; The direction of the end effector is obtained through the gyroscope and accelerometer in the motion capture system; Based on the end effector position and end effector orientation, the actual position and attitude transformation matrix is ​​obtained.

7. The apparatus of claim 5 wherein, It is also used to calculate the position and attitude deviation vector based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, including: Based on the nominal position and attitude transformation matrix and the actual position and attitude transformation matrix, the homogeneous transformation matrix of the end joint relative to the starting joint is obtained. Based on the homogeneous transformation matrix, determine the equation expression containing the position deviation vector, attitude deviation vector, and position and attitude deviation vector; By aligning the first-order differential of the transformation matrix, we obtain the position deviation vector and the attitude deviation vector. Substitute the position deviation vector and attitude deviation vector into the equation expression to calculate the position and attitude deviation vector.

8. The apparatus of claim 5 wherein, The calibration module is used to input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise, specifically for: Based on the least squares method, a least squares matrix equation is generated that includes position and attitude deviation vector, geometric error, and random colored noise. Input the position and attitude deviation vector into the least squares matrix equation to obtain the geometric error and random colored noise.

Images