CN104457741A - Human arm movement tracing method based on ant colony algorithm error correction - Google Patents

Abstract

The invention discloses a human arm movement tracing method based on ant colony algorithm error correction. The human arm movement tracing method comprises the following steps: correcting static errors of an accelerometer by adopting an ant colony algorithm, compensating influences of accelerometer output on system precision, performing attitude computation by adopting a kalman filtering algorithm to estimate an arm joint attitude, and obtaining an arm movement position by taking a shoulder joint as a center node through space three-dimensional deformation. A movement geometric constraint model is introduced to perform angle constraint on shrinkage/stretching of upper and lower arms and turnover of the lower arm, so that the inner rotary angle of the elbow joint is limited within a very small angle; the excursion produced by shake and muscle shrinkage distortion is compensated in the arm movement process by virtue of the constraints, so that the human carton model truly shows the real-time movement of the arm.

Description

A human arm movement tracking method based on ant colony algorithm error correction

technical field

The invention relates to a posture tracking method, in particular to a human arm motion tracking method based on ant colony algorithm error correction.

Background technique

Attitude tracking systems play an increasingly important role in national defense and the national economy. Through the real-time capture measurement of human body posture and data processing and analysis, auxiliary diagnosis and treatment such as three-dimensional gait analysis, artificial prosthesis auxiliary production and correction, spinal curvature correction measurement, and sports training can be effectively realized.

The inertial motion tracking system is mainly composed of MEMS (Microelectromechanical Systems, abbreviated as MEMS) accelerometer, gyroscope and magnetometer. Due to the manufacturing reasons and usage methods of MEMS sensors, there is a certain measurement error in the static output of MEMS accelerometers, which limits its use effect. At present, the most common static correction method used in engineering is the maximum and minimum method. The disadvantage of this method is that it can only estimate the scale factor error and zero position deviation, and it is easily disturbed by random errors. Another commonly used correction method is to form an inertial measurement unit with a gyroscope for testing. Specifically, there is a combined calibration compensation method. This method mainly relies on the reference data provided by the rotating platform for error correction, and can simultaneously calibrate static and dynamic errors. This method has a perfect model and high accuracy of correction effect, but in the correction process, it needs to operate the rotating platform multiple times, the correction process is complicated and cumbersome, and the cost of the rotating platform is beyond the range of ordinary users.

It is known that human motion has certain geometric constraints, and the introduction of these constraints can achieve better motion tracking effects. At present, the commonly used joint-linked bone model is used to track the posture of the human body. The space position between the elbow joint and the wrist joint of the arm is obtained through the transformation between the coordinate systems, and the position of the reference shoulder joint is obtained by using the Lagrangian optimization method through the constraint relationship. This method locates the joint position, but cannot get the corresponding flip angle when the arm is flipped.

Contents of the invention

The purpose of the present invention is to provide a human arm motion tracking method based on ant colony algorithm error correction, which can improve the accuracy of attitude information and improve the problems of poor precision and motion drift in human inertial motion tracking.

To achieve the above object, the tracking method of the present invention includes the following steps:

(1) Solder the three-axis MEMS accelerometer, three-axis gyroscope, three-axis magnetometer and microcontroller on the circuit board through the peripheral circuit to form an inertial sensing unit, and ensure the normal operation and data output of the accelerometer; The sensing unit is fixed on a rotating platform that can rotate 360 degrees around one point in the space to ensure that the circuit board can rotate freely in the three axial directions of the space;

(2) Through the rotating platform that can rotate 360 degrees, the inertial sensing unit is driven to rotate in space manually or automatically, and the rotation speed is less than 10°/s, so as to ensure that the rotation track of the inertial sensing unit covers the entire distribution of the trajectory sphere, At the same time, record the actual output data of the inertial sensing unit and establish a database;

(3) Preprocess the collected data, remove erroneous data, establish the error model of the inertial sensing unit, and design the error estimation algorithm of the accelerometer;

(4) Analyze the output data of the inertial sensing unit in the error model, use the ant colony algorithm to fit the parameters of the error model, perform static correction on the error model, set the acceleration error correction model, and design the error estimation algorithm of the accelerometer , get the best error parameter;

(5) Use the Kalman filter algorithm to calculate the attitude and estimate the position of the arm joints;

(6) Analyze the movement law of the arm posture, propose the geometric constraints of the elbow joint during the arm movement process, establish the spatial coordinate system of the wrist and elbow joints, and obtain the joint position estimation of the arm; establish the plane constraints of joint extension and contraction, Compensation for drift errors;

(7) Fix the inertial sensing unit on the human arm to track the movement posture of the human arm.

This method performs angle constraints on the contraction and extension of the upper and lower arms and the flipping of the lower arms, and obtains the arm motion posture with the shoulder joint as the central node through three-dimensional transformation in space. The angle constraints are realized through the application of the three-dimensional human body model Pose tracking and reproduction.

Wherein, the steps of the ant colony algorithm are as follows: define the number of ant colonies as 100, according to the scope of the independent variable, the random initial colony position, set the objective function value as the size of the pheromone; each ant judges whether it moves according to the size of the pheromone and the degree of movement, the transition probability P is the ratio of the difference between the function value of the next transition point and the minimum function value to the minimum function value, if P is greater than a certain random number, a global search is performed, otherwise a local search is performed; when the original value is not the minimum When the optimal value is obtained, the ant moves to the optimal value position; at the same time, the current point pheromone is updated: where f _i is the function value, is the strength of the pheromone at the current position of the ith ant, and ρ(0<ρ<1) is the pheromone evaporation coefficient. Repeat the above process until the optimal fitting parameters are obtained or a certain number of iterations is reached.

The working principle of the inventive method is roughly as follows:

Firstly, by analyzing the measurement principle and usage method of the MEMS accelerometer, the main error source is determined. On the basis of the original error model, a static correction method of the accelerometer based on the ant colony algorithm is proposed to obtain the optimal acceleration value. Then the Kalman filter algorithm is used to calculate the attitude, and at the same time, the contraction and extension of the upper and lower arms and the flipping of the lower arm are constrained to angle, and the position of the elbow node and wrist node relative to the shoulder is calculated through the skeletal constraints of the arm, and the arm movement process is analyzed. The random drift error caused by the jitter and muscle deformation is compensated to obtain the arm position information.

Compared with prior art, the present invention has following advantage:

1. This method effectively eliminates random errors in the sampling process and improves the measurement accuracy of the MEMS accelerometer;

2. This method does not require a three-axis rotating platform, which reduces user costs. The calculation method is simple and easy to implement, so that the MEMS accelerometer can better meet the actual requirements of the project and provide new options for outdoor testing and other environments;

3. By designing the geometric constraint model of the arm elbow joint, this method compensates the random drift error caused by the shaking and muscle deformation during the arm movement, and can obtain the correct position estimation of the arm joint.

Description of drawings

Fig. 1 is the work flowchart of the inventive method.

Fig. 2 is an optimization flow chart of the ant colony algorithm in this method.

Fig. 3 is a comparison diagram between the original acceleration value and the acceleration amplitude after error correction in the first embodiment.

Fig. 4 is a spatial reference coordinate diagram of the wrist joint and the elbow joint in the first embodiment.

Fig. 5 is a simplified rendering of joint extension and contraction constraints in Embodiment 1.

Fig. 6 is a diagram of the movement track of the wrist joint when the measuring unit is fixed on the wrist joint and the arm bends in the first embodiment.

Fig. 7 is a diagram of the motion track of the wrist joint when the measuring unit is fixed in the middle of the arm and the arm bends in the first embodiment.

Fig. 8 is a coordinate diagram of the node position estimation of the elbow joint in the first embodiment.

Fig. 9 is a coordinate diagram of the node position estimation of the wrist joint in the first embodiment.

Detailed ways

The tracking method of the present invention comprises the following steps:

(1) Solder the three-axis MEMS accelerometer, three-axis gyroscope, three-axis magnetometer and microcontroller on the circuit board through the peripheral circuit to form an inertial sensing unit, and ensure the normal operation and data output of the accelerometer; The sensing unit is fixed on a rotating platform that can rotate 360 degrees around one point in the space to ensure that the circuit board can rotate freely in the three axial directions of the space;

(2) The rotating platform that can rotate 360 degrees through an external device drives the inertial sensing unit to rotate in space manually or automatically, and the rotation speed is less than 10°/s, ensuring that the rotation track of the inertial sensing unit covers all of the trajectory spherical surface distribution, record the actual output data of the inertial sensing unit at the same time, and establish a database;

(3) After preprocessing the collected data, remove the wrong data, establish the error model of the inertial sensing unit, and design the error estimation algorithm of the accelerometer;

(4) Analyze the output data of the inertial sensing unit in the error model, use the ant colony algorithm to fit the parameters of the error model, perform static correction on the error model, set the acceleration error correction model, and design the error estimation algorithm of the accelerometer , get the best error parameter;

(5) Use the Kalman filter algorithm to calculate the attitude and estimate the position of the arm joints;

(6) Analyze the movement law of the arm posture, propose the geometric constraints of the elbow joint during the arm movement process, establish the spatial coordinate system of the wrist and elbow joints, and obtain the joint position estimation of the arm; establish the plane constraints of joint extension and contraction, Compensation for drift errors;

(7) Fix the inertial sensing unit on the human arm to track the movement posture of the human arm.

This method performs angle constraints on the contraction and extension of the upper and lower arms and the flipping of the lower arms, and obtains the arm motion posture with the shoulder joint as the central node through three-dimensional transformation in space. The angle constraints are realized through the application of the three-dimensional human body model Pose tracking and reproduction.

The present invention will be further described below in conjunction with accompanying drawing and specific embodiment:

Embodiment one:

The inertial sensing unit is composed of a microcontroller, a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer. The microcontroller sends attitude information to the personal computer for constraint resolution. In order to balance the processing speed and power consumption, STM32 is selected as the core of the attitude calculation of the attitude sensing unit. The accelerometer is ADXL345, and the maximum measurement range is set to be ±2g; the gyroscope and the magnetometer are respectively L3G4200 from STMicroelectronics and HMC5883 from Honeywell.

Fig. 1 is the work flowchart of the inventive method. The ant colony algorithm is used to correct the static error of the accelerometer and compensate the influence of the accelerometer output on the system accuracy. The Kalman filter algorithm is used for attitude calculation, and the arm joint attitude information is estimated. At the same time, the geometric constraint model of motion is introduced, and the contraction/extension of the upper and lower arms and the flipping of the lower arm are angle constrained. These constraints compensate for the drift caused by shaking and muscle contraction deformation during arm movement.

Figure 2 is a flow chart of the optimization of the ant colony algorithm. The data fitting of the parameters of the accelerometer error model is carried out through the ant colony algorithm, and the ant colony algorithm guides the movement of the ants according to the size of the pheromone in the colony, so as to obtain the optimal solution to the problem to be solved.

The steps of the ant colony algorithm are as follows: define the number of ant colonies as 100, according to the range of independent variables, randomize the initial group position, set the value of the objective function as the size of the pheromone; each ant judges whether it moves and the degree of movement according to the size of the pheromone , the transition probability P is the ratio of the difference between the function value of the next transition point (that is, the pheromone) and the minimum function value and the minimum function value. If P is greater than a certain random number, a global search is performed, otherwise a local search is performed; when the original value When it is not the optimal value, the ant moves to the optimal value position; at the same time, the current point pheromone is updated: where f _i is the function value, is the strength of the pheromone at the current position of the ith ant, and ρ(0<ρ<1) is the pheromone evaporation coefficient. The above process is iteratively repeated until the optimal fitting parameters are obtained or a certain number of iterations is reached.

The best error model parameters obtained by fitting are:

$\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; the\; y</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; z</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0.9685</span>}& \mathrm{<span\; class="notranslate">\; 0.0533</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 0.0188</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 0.0609</span>}& \mathrm{<span\; class="notranslate">\; 0.9743</span>}& \mathrm{<span\; class="notranslate">\; 0.0279</span>}\\ \mathrm{<span\; class="notranslate">\; 0.0044</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 0.0709</span>}& \mathrm{<span\; class="notranslate">\; 0.9841</span>}\end{array}\right]\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}}\end{array}\right]<span\; class="notranslate">\; -</span>\left[\begin{array}{c}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 4.6434</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 10.4547</span>}\\ \mathrm{<span\; class="notranslate">\; 33.3557</span>}\end{array}\right]$

Figure 3 is a comparison chart between the original acceleration value and the acceleration amplitude after error correction. By continuously sampling the output of the accelerometer, the sampling points cover all points on the rotating sphere of the accelerometer to ensure the precision and accuracy of parameter estimation. The ideal output of the accelerometer is 1g, and the average output value of the actual accelerometer reaches 1.06g. The static error correction is carried out through the ant colony algorithm. The average value of the acceleration value after correction is 1.002g. The correction effect is obvious, as shown in Table 1. (Among them, "ACO" is the abbreviation of Ant Colony Algorithm)

Table 1 Gravity acceleration value before and after correction amplitude comparison

Fig. 4 is a spatial reference coordinate diagram of the wrist joint and the elbow joint. definition is a 3×3 rotation matrix indicating the transformation of the attitude from the sensor coordinate system to the geographic coordinate system:

${\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; w</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; w</span>}}{\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}$

where v ^w and v ^b denote the linear acceleration in the geographic coordinate system and the sensor coordinate system, respectively. state of the next moment It can be updated by:

${\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; w</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; w</span>}}\mathrm{<span\; class="notranslate">\; S</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; )</span>$

Among them, S(ω ^b )=[ω ^b ×] is an antisymmetric matrix, which represents the cross product operation of angular velocity estimation. Obtaining the rotation matrix, the acceleration vector in the geographic coordinate system can be deduced:

${\mathrm{<span\; class="notranslate">\; a</span>}}^{\mathrm{<span\; class="notranslate">\; w</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; w</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}$

in is a 3×3 rotation matrix, and a ^b represents the acceleration vector in the sensor coordinate system. By obtaining the acceleration and Euler angle expressed in the geographic coordinate system, the position vectors of the elbow and wrist joints in the geographic coordinate system can be known, assuming that the length of the upper arm is L ₁ and the length of the lower arm is L ₂ . Under static conditions, the X axes of the two inertial sensors are collinear with the orientation of the upper and lower arms. During the movement, the position of the elbow joint P _e is calculated by the following formula in the coordinate system based on the shoulder:

P _e =R _es P _e0

Where R _es is the rotation matrix of the upper arm, P _e0 =[L ₁ ,0,0] ^T . The position of the wrist joint P _w can be calculated in the coordinate system based on the shoulder:

P _w ＝R _we P _w0 +P _e

Where R _we is the rotation matrix of the lower arm, P _w0 =[L ₂ ,0,0] ^T .

Figure 5 is a simple effect diagram of joint extension and contraction constraints. The internal rotation and external rotation of the elbow joint are basically limited, that is, the internal rotation angle а is limited to a small angle, and at the same time, it can be concluded that the x-axis of the lower arm is parallel to the x-y plane of the upper arm, that is, the z-axis of the upper arm Vertical, represented by the following point multiplication formula:

${\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; u</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; u</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}$

where _ωt is Gaussian white noise, and are the x-axis vector of the lower arm and the z-axis vector of the upper arm in the limb coordinate system, respectively. At the same time, for the extension and contraction of the arm, the movement is also limited to a specific angle, and the retraction angle of the arm is in a specific range, that is, the angle between the x-axis of the upper arm and the x-axis of the lower arm is limited. Can be expressed as follows:

${\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; arccos</span>}\frac{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; u</span>}}}{<span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; u</span>}}<span\; class="notranslate">\; |</span>}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}$

The above formula is only a necessary but not sufficient condition, because the x-axis of the lower arm is parallel to the x-y plane of the upper arm, another constraint relationship can be obtained:

${\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; arccos</span>}\frac{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; \&CenterDot;</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; u</span>}}}{<span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; u</span>}}<span\; class="notranslate">\; |</span>}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}$

where γ represents the angle between the x-axis of the lower arm and the y-axis of the upper arm. When the upper arm does not move and the lower arm is driven to flip the palm, the flip angle is within a certain range. In medical operations, the range of change is generally small and smooth. The range of change can be regarded as the z-axis of the upper arm and the z-axis of the lower arm. The angle between the axes, expressed as follows:

${\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; arccos</span>}\frac{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; u</span>}}}{<span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; \&CenterDot;</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; u</span>}}<span\; class="notranslate">\; |</span>}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}$

Firstly, the IMU measurement unit is used to test the attitude measurement results of a single limb and the influence of the installation position of the sensor on the attitude measurement results. Fix the attitude measurement unit 3cm close to the wrist joint and elbow joint, keep other parts of the body still, the position of the shoulder joint can be regarded as fixed, as the origin of the position, and sample the sensor data at a frequency of 25Hz. The measurement accuracy of a single IMU measurement unit affects the attitude tracking accuracy of the entire system.

Fig. 6 is a diagram of the trajectory of the wrist joint when the inertial sensing unit is fixed at the position of the wrist joint and the arm bends. The measured lengths of the upper arm and the lower arm of a normal human body are both about 30cm. To facilitate the comparison of the results, L ₁ =30cm and L ₂ =30cm are taken here. First, fix the inertial sensing unit at a distance of 3cm from the wrist joint, keep the upper arm still, stretch and contract the lower arm at a slower speed, repeat the movement 4 times, and measure the movement trajectory of the wrist joint.

Fig. 7 is a diagram of the trajectory of the wrist joint when the inertial sensing unit is fixed in the middle of the arm and the arm is bent. Fix the inertial sensing unit in the middle of the lower arm, keep the upper arm still, stretch and contract the lower arm at a slower speed, repeat the movement 4 times, and measure the trajectory of the wrist joint.

Fig. 8 and Fig. 9 are the coordinate diagrams of the node position estimation of the elbow joint and the wrist joint. The attitude measurement unit is fixed on the arm, and the position of the elbow joint and the wrist joint in the reference coordinate system centered on the shoulder node is measured. Keeping the body still, the arm moves along the edge of a square with a fixed size on the table, the Euler angles of the upper arm and the lower arm are obtained by the attitude measurement unit, the geometric constraints of the arm are introduced, the points with large attitude errors are filtered out, and the elbow angle is calculated. Coordinate positions of joints and wrist joints. Four testers were tested at the same position to measure the spatial position of the elbow joint and wrist joint. The arm moved along a fixed path twice at a constant speed, and the movement period was 10s, that is, 250 samples. Figure 8(a), Figure 8(c), and Figure 8(e) are the X, Y, and Z coordinate values of the elbow joint, and Figure 9(b), Figure 9(d), and Figure 9(f) are the wrist joint X, Y and Z coordinate values. It can be seen from the figure that the estimated positions of different testers change along a fixed path, but due to the influence of exercise habits and arm shaking during exercise, they do not completely match the design path. By selecting the average value of four groups of position estimates, the error average value, standard deviation and standard deviation of the elbow joint and wrist joint are calculated respectively, as shown in Table 2:

Table 2 Comparison of joint position estimation errors

From the above analysis, it can be concluded that the average error of the four groups of fixed square experiments is within 1cm, which correctly represents the estimated position of the joint. The standard deviation and standard deviation indicate that the error fluctuation is very small, which meets the stability requirements of the motion tracking system.

The above-mentioned embodiments are only descriptions of preferred implementations of the present invention, and are not intended to limit the scope of the present invention. All such modifications and improvements should fall within the scope of protection defined by the claims of the present invention.

Claims (3)

1. based on human arm motion's tracking of ant group algorithm error correction, it is characterized in that, described tracking comprises the steps:

(1) 3 axis MEMS accelerometer, three-axis gyroscope, three axle magnetometers and microcontroller are welded on circuit board by peripheral circuit form inertia sensing unit, and ensure that the normal running of accelerometer and data export; Inertia sensing unit is fixed on and on the rotatable platform of 1: 360 degree, space rotation, can ensures that circuit board axially can rotate freely three of space;

(2) by can 360 degree rotations rotatable platforms, drive inertia sensing unit at Space Rotating with manual or automated manner, and rotational speed is less than 10 °/s, ensure that the rotational trajectory of inertia sensing unit covers all distributions that accelerometer rotates sphere, record the actual output data of inertia sensing unit simultaneously;

(3), after the data of collection being carried out pre-service, remove misdata, set up inertia sensing elemental error model, the error estimation algorithm of design acceleration meter;

(4) to the output data analysis of inertia sensing unit in error model, ant group algorithm is adopted to carry out parameter fitting to error model, static modification is carried out to error model, the VEC of acceleration is set, the error estimation algorithm of design acceleration meter, obtains best error parameter;

(5) adopt Kalman filtering algorithm to carry out attitude algorithm, estimate arm joint position;

(6) analyze the characteristics of motion of arm attitude, propose the geometry constraint conditions of elbow joint in arm motion process, set up the space coordinates of wrist, elbow joint, show that the joint position of arm is estimated; Set up the plane restriction of joint extension, contraction, offset drift error;

(7) inertia sensing unit is fixed on human arm, human arm motion's attitude is followed the tracks of.

2. a kind of human arm motion's tracking based on ant group algorithm error correction according to claim 1, it is characterized in that: angle restriction is carried out in the upset for the contraction of upper and lower arm, stretching, extension and underarm, converted by space three-dimensional, obtain the arm motion attitude of node centered by shoulder joint, described angle restriction realizes Attitude Tracking and the reproduction of arm by human 3d model application.

3. a kind of human arm motion's tracking based on ant group algorithm error correction according to claim 1, it is characterized in that, described ant group algorithm step is as follows: definition ant group number is 100, according to the scope of independent variable, random initial population position, if target function value is pheromones size; The size of every ant foundation pheromones judges whether it moves and mobile degree, transition probability P is the difference of next branchpoint functional value and minimum function value and the ratio of minimum function value, if when P is greater than certain random number, carry out global search, otherwise carry out Local Search; When original value is not optimal value, then ant is moved to optimal value position; Upgrade current point pheromones simultaneously: wherein f _tfor functional value, be the intensity of i-th ant current location information element, ρ (0 < ρ < 1) is pheromones evaporation coefficient, to said process iteration, until obtain optimal fitting parameter or reach certain iterations.