Abstract
In this paper we focus on the human arm motion capture, which is motivated by the requirements in physical rehabilitation and training of stroke patients in the same way as monitoring of elderly person activities. The proposed methodology uses a data fusion of low-cost and low-weight MEMS sensors jointly to an a priori knowledge of the arm anatomy. The main goal is to estimate the arm position, the anatomical movements of the shoulder and its accelerations. We propose a discrete optimization based-approach which aims to search the optimal attitude ambiguity directly without decorrelation of ambiguity, and to computing the baseline vector consequently. The originality of this paper is to apply the discrete optimization to track the desired trajectory of a nonlinear system such as the Human Movement in the presence of uncertainties. The global asymptotic convergence of the nonlinear observer is guaranteed. Extensive tests of the presented methodology with real world data illustrate the effectiveness of the proposed procedure.
1. Introduction
Many different disciplines, such as computer character animation or virtual reality use motion capture systems to capture movement and posture of human body. However, the human motion capture can be used in the care of individuals with physical recovery, rehabilitation, and training needs. The study of human-robot interaction (HRI) for assistive robotics applications is a new, growing, and increasingly popular research area at the intersection of a number of fields, including robotics, control, medicine, psychology, neuroscience, and cognitive sciences. In contrast to interactive robotics, which aims to entertain and create simple basic relationships with human users, assistive robotics focuses on aiding human users with special needs in their daily activities [1].
Robotic technology has great potential to benefit the lives of people with disabilities [2]. Not only in the obvious role as a manipulator, whether mounted on a desk, wheelchair, mobile base, or body worn, but also as an aid to mobility, education, and communication. Assistive robotics is the bringing together of several technologies with the needs of people with various disabilities; in this paper, we present our research approach in assistive robots via motion capture.
During the rehabilitation process, the movement of stroke patients needs to be localized and learned so that incorrect movements can be instantly modified or tuned. Therefore, tracking these movements becomes vital and necessary during the course of rehabilitation. The most popular tracking systems are the mechanical trackers, the active magnetic trackers, and the optical tracking systems [3].
Mechanical trackers systems [4] utilize an exoskeleton that is attached to the articulated structure to be tracked. Goniometers within the skeletal linkages measure joint angles that are used with kinematics algorithms to determine body posture. While mechanical trackers are precise, they are worn; moreover, the alignment of the goniometers is difficult. Active magnetic tracking systems [5] determine both position and orientation by using small magnetics sensors mounted on a rigid body to sense a set of generated magnetic fields; this kind of systems have several weaknesses, namely, field distortion and limited measured volume. Optical tracking systems [6] may be separated into two basic categories: pattern recognition and image-based systems. Pattern recognition systems sense an artificial pattern of lights and use this information to determine position and/or orientation. Image-based systems determine position by using multiple cameras to track predesignated points on moving objects within a working volume. Although optical tracking systems provide accurate orientation and position information, they have some important limitations. Limitations include occlusion (line of sight) problems whenever a required light path is blocked, interference from other light sources, limited measured volume and the most important, high cost.
The use of (microelectromechanical Systems) (MEMS) sensors in human movement analysis has gained substantial ground. Although accelerometers and rate gyros have been used in applications such as seismic analysis, aerospace navigation, and robotics, recently they are used in application such as monitoring of human activities [7, 8] and measurement of neurological disorders [9]. In other, the advent of motion inertial capture systems offers a convenient means for acquiring realistic motion data [10]. Due to the success of such systems, realistic and highly detailed motion clips are commercially available and widely used for producing visually convincing animations of human-like 3D characters in a variety of applications, such as animation films and video games.
Research in human motion capture has concentrated on the articulated-model based approach in order to produce a general full-body tracker enough to handle realistic real-world applications. However, the problem with using articulated models is the high dimensionality of the configuration space and the exponentially increasing computational cost that results.
In order to circumvent the problem that represent the use of mechanical, active magnetic, and the optical tracking systems, a technique based on inertial tracking system can be used. The technique consists of placing MEMS sensors (rate gyros, accelerometers, and magnetometers) units on each body segments to be tracked. The articulated structures are describe as nonrigid objects. However, each structure separately can be seen as rigid body [3]. The lengths of body segments between rotational joints therefore remain constant over time, connected by articulations having each one one or more degrees of freedom (DOF). The measurements are made in reference to the local axes roll, pitch, and yaw. The clinical reference system provides anatomically meaningful definitions of main segmental movements (e.g., flexion-extension, abduction-adduction, or supination-pronation) in the case of the arm, the reference point is the shoulder (see Figure 1).
Consequently, using this technique, the human motion capture is in the framework of the attitude estimation, from three rate gyros, three accelerometers, and three magnetometers sensors, mounted orthogonally. This combination is also known as an attitude and heading reference system (AHRS) [11].
In the last decade, the attitude estimation problem has been applied to new fields such as, for example, ground and aerial robotics [12, 13]. The attitude estimation is carried out by means of data fusion from rate gyros, accelerometers, and magnetometers. Actually, rate gyros provide continuous attitude information with good short-term stability when their measurements are numerically integrated. However, since rate gyros measurements are affected by drifts, the attitude estimation based on these sensors slowly diverges from the real attitude. On the other hand, linear accelerometers measure the vector sum of acceleration and gravitational acceleration in sensors coordinate. When the gravitational acceleration component is sensing dominant, it provides inclination information. Thus, it can be used to correct the drifted orientation estimate from rate gyros. Since accelerometers cannot detect rotation about the vertical axis, magnetic sensors can be added, which allows to sense the earth magnetic field, and they can thus be used to correct drift of the rate gyro about the vertical axis, [14]. In general, the idea has been to implement filters in which accelerometers and magnetometers are used for low frequency components of orientation and rate gyros for to measure faster changes in orientation. In the above cited approaches, the data fusion is carried out by means of the implementation of extended Kalman filters (EKF). The major feature of EKF's concerns the ability to fuse signals acquired from different sensor types. However, the linearization process renders difficult to guarantee the global convergence at the truth attitude. Moreover, since the sensors measurements noise encountered in practice is in general non-Gaussian the filter behavior can become poor. Recently, nonlinear approaches for the attitude estimation using accelerometer, magnetometers and rate gyros have been proposed. In [15], the authors propose a nonlinear observer, and they exploit the group in order to estimate the attitude matrix. In the attitude nonlinear observers approach, the convergence of the attitude error to zero is proved in a Lyapunov sense.
For discrete element modeling (DEM), though, there are typically no large angle displacements between time steps, which relaxes many of the constraints of the algorithms just discussed. In DEM and several other particle methods, the time step must be chosen to be relatively small to accurately resolve highly nonlinear, inter-body contact, and explicit integration schemes are often used to update the motion of the discrete elements. Indeed, implicit time-stepping algorithms have been shown by [16] to be often suboptimal in discrete element applications.
In all these mentioned approaches, the acceleration component is modeled as a first-order low-pass filtered white noise process, that is, the accelerometer is used as an inclinometer. Thus, a problem arises when the acceleration is not small in comparison to the gravity. In fact, these approaches do not consider the problem of attitude estimation which takes the body nonweak accelerations into account. Few publications have treated this problem [17, 18], where the robot's Kinematic model is taken into account and ground-directed rate sensors are added. In [19], a fuzzy logic expert rule-based system is designed to identify the status of vehicle motion and fuse the data from these different sensor modalities. Unfortunately, these algorithms are a combination of linear Kalman Filters and Extended Kalman filter, and as it has been mentioned, this fact renders difficult to guarantee global convergence.
The goal of assistive technology is to develop advanced technical aids for promoting independent living and improving quality of life of persons who have chronic or degenerative impairment in motor, sensory, communication, and/or cognitive abilities. The aim of this paper is to describe the results of our research activities in robotics with applications to people affected by disability and by older people with degenerative disorders due to the natural course of aging.
In the present work, our typical capture configuration relies primarily on the arm. The motivation arises of the requirement in physical rehabilitation and training of stroke patients in the same way as monitoring of elderly person activities which may assist clinicians in the early recognition of potential pathology. The objective is to estimate the arm position and the movements of the shoulder (see Figure 4) with respect to an inertial frame, represented by the human body. Therefore, a module containing three rate gyros, three accelerometers, and three magnetometers assembled in tri-axis, is positioned in the extreme of the arm, in the articulation proximal of the hand. The attitude for the coordinate reference sensors is estimated. The combination of this information jointly to a priori knowledge of the anatomy of the arm makes possible to obtain information on the posture of arm and the anatomical movements of the shoulder. In this first stage, the elbow flexion-extension is not considered.
The attitude estimator uses quaternion representation. Two approaches are jointly used, namely, a nonlinear observer and a discrete linear quadratic minimization technique. The nonlinear observer is performed in order to produce an estimate of the gyro bias and of the attitude quaternion. Thus, no assumptions of the weakness (or not) of the accelerations are done, and no switching procedure from one observer to another one is necessary. The main advantage of the approach presented in this paper compared to others approaches is that the estimated attitude remains valid even in the presence of high accelerations over long time periods. Moreover, the gyro bias is estimated online, and the algorithms and methodology can be embedded in micromechanic system or robotic systems.
In this paper, a discrete linear quadratic (DLQ) optimization is proposed to track the desired trajectory. The parameters are optimized such that the tracking error is minimized since it is a model-free approach. However, the particle swarm optimization requires a repeatable certain model and the optimal control design is an off-line type.
The present paper is organized as follows. In Section 2, a mathematical background is given. The problem statement is formulated in Section 3. The nonlinear observer is presented in Section 4. The use of the discrete optimization algorithm together with the nonlinear observer is explained in Section 5. Experimental results are given in Section 6. The paper ends with some concluding remarks given in Section 7.
2. Mathematical Background
As mentioned in the introduction, the attitude of a rigid body can be represented by an unit quaternion, consisting of an unit vector , known as the Euler axis, and a rotation angle about this axis. The quaternion is then defined as follows: where , and are known as the vector and scalar parts of the quaternion, respectively. In human motion tracking applications, the unit quaternion represents the rotation from an inertial coordinate system located at some point in the space (for instance, the earth frame) to the body coordinate system located on the center of the mass of a rigid body.
If is a vector expressed in , then its coordinates in are expressed by: where and are the quaternions associated to vectors and , respectively, denotes the quaternion multiplication, and is the conjugate quaternion of follows defined as: The rotation matrix corresponding to the attitude quaternion is computed as follows: where is the identity matrix and is a skew symmetric tensor associated with the axial vector as follows: Thus, the coordinate of vector expressed in the frame is given by: The quaternion attitude error used to quantify the mismatch between two attitudes and is computed by: Denoting by the angular velocity vector of the body frame relative to the inertial frame , expressed in , the kinematics equation is given by:
2.1. Modeling Sensors
2.1.1. Rate Gyros
The angular velocity is measured by the rate gyros, which are supposed to be orthogonally mounted. The output signal of a rate gyro is influenced by various factors, such as bias drift and noise. In the absence of rotation, the output signal can be modeled as the sum of a white Gaussian noise and of a slowly varying function. Since an integration step is required in order to obtain the current attitude quaternion (9), even the smallest variation of the rate gyro measurement will produce a wrong estimation of the attitude. The bias is denoted by , belonging to space . The rate gyro measurements are modeled by [20]: where and are supposed by Gaussian white noises and is a diagonal matrix of time constants. In this case, the constant has been set to s. The bias vector will be estimated online, using the observer presented in the following section.
2.1.2. Accelerometers
Since the 3-axis accelerometer is fixed to the body, the measurements are expressed in the body frame . Thus, the accelerometer output can be written as follows: where and are the gravity vector and the inertial accelerations of the body, respectively. Both are expressed in frame . m/sec2 denotes the gravitational constant, and is the vector of noises that are supposed to be white Gaussian.
2.1.3. Magnetometers
The magnetic field vector is expressed in the frame; it is supposed to be . Since the measurements take place in the body frame , they are given by: where denotes the perturbing magnetic field. This perturbation vector is supposed to be modeled by Gaussian white noises.
3. Problem Statement
The objective is to estimate the arm attitude and the anatomical movements of the shoulder, namely, extension-flexion, abduction-adduction, and Internal-External rotations (see Figures 2 and 4). The elbow Flexion-Extension is not considered.
A module containing three rate gyros, three accelerometers, and three magnetometers assembled in tri-axis is positioned in the shoulder and in the extreme of the arm, in the articulation proximal of the hand (see Figure 3). The problem becomes in the attitude estimation of the coordinate reference module sensors ().
(a) Anatomical movements of the shoulder
(b) Model of the human body
The magnetometers sense the earth magnetic field in the body frame . Their measurements are noted (Figure 2). The accelerometers measure the sum of all arm acceleration and of the gravity field. Then, the influence of the arm accelerations must be removed from the accelerometer measurements. Their measurements are expressed in and are denoted by . The rate gyro measures the rotational motion. The measurements present a good short-term stability and a low noise level. However, rate gyro measurements are affected by inherent drifts which result in slow divergence of attitude over time, since an integration step is required. Hence, the bias of the rate gyro must be estimated online and will be subtracted to the rate gyro measurements.
Once that the attitude of the coordinate reference module sensors is obtained, it will be combined with a priori knowledge of the anatomy of the arm making possible to obtain information on the posture of arm and the movements of the shoulder ().
The limitations in rotation of the shoulder are summarized in the Table 1 [21].
3.1. Human Model
For the application and its model, this is based on an articulated arm, because one seeks the motion capture of the human movement. For this purpose, a model is extracted from the robots denominated “light,” robots in which all the actuators are attached within the robot's armor in order to limit the movement of the masses of the different elements. Although the developed tools are valid for all the parallel types of robots, the model is based on robots of rotular type ( for rotation) that offer a special interest for this particular application, since the joints that one wants to consider of the human body can be modeled (cf. Figure 4) by pivots joints, and in the shoulder, it can be modeled via prismatic-rotule articulations 2.
4. Nonlinear Attitude Observer
The attitude nonlinear observer that includes the bias and the error update is given by: where has been defined in (11) and , are positive matrixes. is the prediction of the attitude at time . It this obtained via the integration of the kinematics equation (14) using the measured angular velocity , the bias estimate , and which is the vector part of the quaternion error . Remember that measures the discrepancy between and the pseudomeasured attitude (16). In this paper, is obtained thanks to an appropriate treatment of the accelerometer and magnetometer measurements via a discrete optimization, and it will be explained in the next section.
Combining (9), (11), and (14), the error model is expressed as: where and . The system (16)-(17) admits two equilibrium points and . This is due to the fact that quaternions and represent the same attitude. From (1), one obtains that is, there is only one equilibrium point in the physical 3D space. The almost global asymptotically convergence of the error to zero and consequently the convergence of to the real is given by: where is the “true” attitude quaternion of the rigid body. Thus, the convergence is guaranteed if and only if
Theorem 1. Consider the equilibrium states of the system (16)-(17) and let be the measured angular velocity. Then, the equilibrium point is globally asymptotically stable.
Proof. Consider the candidate Lyapunov function , which is positive definite, radially unbounded and which belongs to the class : The derivative of (21), together with (17) and (16), is given by: Since and , it comes that Thus, . Consequently, . However, if the initial conditions of the error model lie everywhere except on the two equilibrium points, the observer error will asymptotically converge to the point , where . In fact, this equilibrium point is considered as an attractor point, whereas the point is considered as an repeller point. From (21), it can be noticed that if the states of the error model are at the repeller equilibrium point, they remain in this point for . Nevertheless, if any small perturbation is present (maintaining the condition ), it will produce a decrease in the value of , and since for any point (except the equilibrium points where ), one obtains , that concludes the proof.
Remark 2. In practice a zero physical attitude error (either or ) is desired in a minimum time and with minimum effort, starting from any initial condition. In fact, the equilibrium point can be considered as an attractor equilibrium point for the error model if the measurement update is used. Thus, using with instead of (14)–(25), it is possible to ensure that the rotation with the smallest angle or is chosen.
5. Discrete Optimization of the Pseudomeasurement Quaternion and Acceleration Estimation
The attitude estimator uses quaternion representation. Two approaches are jointly used, namely, an estimation with a constraint least-square minimization technique and a prediction of the estate at the instant . The prediction is performed in order to produce a pseudo-estimate of the accelerations and the attitude quaternion. This prediction is driven by a estate which is obtained from the quaternion propagated through the kinematic equation and the one obtained via the constraint minimization problem.
The pseudomeasurement quaternion is computed from the accelerometer and magnetometer measurements. Actually, the accelerometers are sensitive not only to the gravity vector but also to the body accelerations. In the case when the accelerations are not weak, the attitude computed using these direct measurement sensors is far from the true attitude vector. Therefore, an optimization problem is formulated which is divided in three steps.
Firstly, one seeks to estimate the inertial body accelerations from accelerometer measurements and the quaternion obtained from (14). In this way, the quaternion that is obtained by the estimation with a constraint least square is insensitive to the body accelerations. Thus, no assumptions of the weakness (or not) of the accelerations are done, and no switching procedure from one model to another one is necessary. Therefore, the main advantage of the approach presented in this paper compared to others approaches is that the estimated attitude remains valid even in the presence of high accelerations over long time periods. The measures obtained by the accelerometers and the magnetometers are modeled by (12) and (13), respectively. Thus, the following cost function is proposed In fact, this optimization problem has an explicit solution which is obtained by the following:
Once the acceleration vector is estimated, it is removed from the accelerometer measurements in order to use only the information of the field gravity vector projected in the body frame . Therefore, an optimal attitude quaternion is found by means of an optimization criteria that takes into account the evolution of the attitude state via the determination of in the function . The minimum error is chosen by taking into account the prediction of the state and the coefficients weight for the estate and the measures estimated (MesEstimated = MS) at the instant , that is, subject to the constraint
This nonlinear least-square problem can be easy solved using a quasi-Newton's method instead of classical Gauss-Newton's one. In fact, quasi-Newton's method shows faster and more stable local convergence properties than Gauss-Newton's one [22]. This method requires the knowledge of the first and the second derivative of the function to minimize with respect to each element , and . The process of estimation and prediction needs the determination of his gradient. In order to compute such a gradient, the following definitions will be needed.
The gradient of the quaternion is determined via the following: For the acceleration, his gradient is given as follows: Finally, the gradient is given by: This discrete optimization leads to a simplified calculation of the Hessian:
Remark 3. For the prediction's process of , the cubic spline method can be applied.
The minimization of (26) and (28) using numerical software and a linear-quadratic regulator ad hoc function, with the knowledge of the derivative (faster), will take a mean of 0.85 sec, which can be compared to the 0.025 sec that are needed for the Newton's algorithm. This result could be even more improved with an optimization of the computation. All computations were done on a PC at 1.6 GHz with 1Go Ram.
It can be proved that the problem (28) admits two global solutions, namely, and [3]. Actually, both and give the same orientation of the rigid body in the physical space. The optimization routine is initialized by which is obtained by the integration of (14). Then, thanks to an appropriate choice of the sampling period, the quaternion obtained by solving (28) is always nearest to . Therefore, does not exhibit jumps as it is observed with others techniques used to compute the quaternion attitude from accelerometer and magnetometer measurements. Moreover, the chosen algorithm guarantees the quaternion norm constraint.
The performances of the proposed method are similar to those of the Extended Kalman Filter (multiplicative and additive); the results are depicted in Figure 5. However, for the extreme errors, the convergence rate for our estimation-prediction is higher.
(a)
(b)
(c)
The schema describing the full estimator () is given in Figure 6. The algorithm in the upper box attempts to find the “best” quaternion that relates the gravity field and the earth magnetic field measurements in the body coordinate frame to well known values in the earth coordinate frame. Remember that the inertial body accelerations have been removed from the directs accelerometer measurements. This pseudomeasured quaternion is compared to obtained by the integration of the dynamics equation of the observer (24), in order to compute the error .
Remark 4. In general, a nonlinear observer is not robust to measurement disturbance in the sense that arbitrarily small disturbance may results in a blowup of error state.
Recently in [11] we have shown that the error dynamics in the proposed attitude observer is passive. Motivated by this fact, the robustness of the proposed observer can be showed, considering that the proposed observer is input-to-state stable when the measurement disturbance is seen as input and the error state as the state [23].
5.1. Simulated Data
In this section, some simulation results are presented in order to illustrate the performance of the proposed method. A rigid body with low moment inertia is taken as the experimental system; in our case the movement of the shoulder is carried out. In fact, the low moment of inertia makes the system vulnerable to high angular accelerations which proves the importance of applying the method.
In order to verify the performance of the proposed discrete optimization, several simulation cases are carried out. In this paper only one of them is shown.
The movement of the shoulder is oscillatory, for the estimation and prediction algorithm, the conditions point is “far” from the theoretical point in orden to observe the convergence of the methodology proposed. The simulated acceleration is depicted in Figure 7. As can be seen, it exhibits transients and steps values, and the discrete optimization (estimation and prediction) is shown in Figure 7. It is evident that cannot be considered as weak.
(a) Acceleration: simulated, estimated, and predicted
(b) Quaternion: simulated, estimated, and predicted
For the case of the quaternion estimated and predicted, the results of the methodology proposed are shown in Figure 7, and as expected the attitude converges to the movement simulated.
6. Application to Real Data
In this part of the work, some numerical implementations of the observer are presented. Moreover, the estimation methodology proposed in this work is implemented and evaluated with real data, in order to assess its effectiveness.
The discrete-time model that describes the attitude kinematics is expressed by
The vector is the quaternion at the time instant , where is the system's sampling period. Observe that (34) is valid provided that the angular velocity measured at the time instant is assumed to be constant in the time interval . The process of computing the initial condition is usually called alignment. Once (34) is solved, the attitude matrix can be updated via (5).
The attitude estimate is used twice during positioning estimation. The accelerometer senses the body's own (nongravitational) acceleration and the projection of the gravitational acceleration (12).
The next step requires integration of to derive the position estimate A numerical integration routine such as the trapezoidal rule can be used to solve (35) numerically.
The normalized quaternion moves the unit sphere along an arc connecting to . Finally, the desired expression of the interpolated quaternion, which fulfils the initial and end conditions for a stride, is as follows:
The gravity-compensated acceleration is then single time integrated; the initial and end conditions are imposed to avoid drift (null velocity), before positioning estimation.
The angular velocity is obtained by finite differences at the instants and ( estimation instant) as follows:
A commercial attitude unit (CAU) is used to acquire the sensor data described in Section 3. This CAU may provide the Euler’s angles. Remember that the attitude estimate is computed using a unit quaternion formulation. For comparison purpose, the estimated quaternion is converted into Euler's angles.
During this test, the CAU is placed in the hand and in the shoulder (Figure 3). The Euler's angles provided by the CAU (named CAU angles) together with the 9 measurements are recorded. The CAU angles are compared to the angles corresponding to the attitude quaternion estimated with the proposed methodology (named angles). The movement of the hand is composed of circular movements in each axe and the movement of the shoulder are Flexion-Extension, Abduction-Adduction, and Internal-External. As can be seen in Figure 8, the states provided by the algorithm in Figure 6 converge to their expected values. The roll angle remain nearly constant which is consistent with the movement that has been performed. Note that the comparison between the CAU angles and the angles is difficult because the CAU cannot be considered as a reference attitude measurement system. Actually, no information about the algorithm embedded in the CAU is available. Moreover, the CAU system does not directly provide the body accelerations. In Figure 8, the accelerations are given in the inertial frame . As expected, the components of are consistent with the circular movement.
The sensor module which combines three angular rate gyros, three orthogonal DC accelerometers, and three orthogonal magnetometers was designed and constructed; the noise characteristic of the senors is show in Table 2. The 9 orthogonal sensors measurements are obtained at an sample rate of 60 Hz, and they are transmitted to the PC by means of the serial port. In order to have a friendly interface, the acquisition protocol is put in a dynamic-link library (DLL) which can be used during off-line analysis or during real-time application using any numerical software and simulation environment platform.
6.1. Trial Description
The analysis performance of the proposed observer can be split into two stages. The ability of the observer to estimate the rate gyro bias and the quality of the attitude (orientation) estimation towards nonweak accelerations. In order to show the effectiveness of the observer, a trial was realized, the sensors data were recorded, and an off-line analysis were carried out. The trial is described as follows: (1) The sensor module (SM) was placed to a well-known orientation (the SM is still in the horizontal, and it is headed toward the magnetic north) which correspond to shoulder angular position . (2) The SM is still at for 10 sec. (3) The SM is turned in all directions to different angular velocities for 20 sec. Shoulder Extension-Flexion, Abduction-Adduction, and Internal-External rotation .(4) The SM is placed at its initial angular position such that the final and initial orientations are identical. (5) For all the simulation time span, the signal m/sec2 is added to accelerometers channels (that is done in order to show the ability of the proposed approach to estimate the acceleration and in the same way to depict the insensibility to nonweak acceleration). (6) For all the simulation time span, the vector was added to the vector of the rate gyro measurements, in order to test the ability to estimate and compensate the rate gyro bias.
The initial conditions for the observer state are: and .
In Figure 9, the estimated quaternion and the pseudomeasured quaternion associates to the shoulder angular position are depicted. As expected, the attitude estimate reaches the true attitude (which in this case it is well known and represented by: ) in suitable time for practical implementation. In this trial, after the shoulder movements the final shoulder angular position coincides with the initial ones. It can be noticed that despite the noise, drift bias and nonweak acceleration introduced, the evolution of the quaternion estimated reaches the values of the initial attitude .
| (a) Quaternion estimation: (blue) (red) |
| (b) Added bias to the rate gyro channels |
The ability of the observer to estimate the bias is showed in Figure 9, where the added bias vector and its estimate are depicted.
The acceleration actuating in the sensor coordinate system (the signal added to accelerometers channels), expressed in the inertial coordinate system, is depicted in Figure 10. That demonstrates the capabilities of the proposed observer to estimate together the attitude and the acceleration. This last fact allows to obtain an attitude quaternion insensitive to nonweak accelerations.
Finally, the estimated movements of the shoulder: Extension-Flexion, Abduction-Adduction, and Internal-External rotation are showed in Figure 11. These results show the effectiveness of the estimation schema.
The attitude estimation observer proposed in this paper has been implemented in language , where efficient methods have been applied for computing the quaternion-based attitude. The implemented observer is also put in a dynamic-link library (DLL) which can be used during real-time application, as show in Figure 12, where we can see the virtual human model tracks the movement of the person and move the robot in the laboratory (remote assistive robot Figure 12).
(a) Anatomical movements of the shoulder: Real-time estimation
(b) Interaction with the robot in the laboratory
7. Conclusion and Future Works
In this paper, a new approach to estimate together attitude and accelerations of a body is presented. The orientation of the rigid body has been parameterized by unit quaternion in order to avoid the singularities of the Euler's angles. The attitude estimation unit is made of nine sensors, namely, a triaxis accelerometer, three magnetometers and three rate gyros that are orthogonally mounted. A nonlinear observer is implemented, and its convergence is proved. A quaternion pseudomeasurement is obtained from the accelerometer and magnetometer measurements using a discrete optimization technique with constraints. Note that the influence of the body accelerations is removed from the accelerometer measurements. Thus, the quaternion pseudomeasurement does not depend on the acceleration values. Therefore, the classical hypothesis “ is weak”, is no more necessary. Moreover, only one observer is used and it does not depend on the acceleration amplitude.
The proposed nonlinear observer together with its pseudomeasurement computation is tested on simulated and real data. The results obtained are really encouraging, because the observer error is very small with a low computational cost. Moreover, a version of the algorithm for attitude and acceleration estimation is applied to interacts with a virtual 3D model of robot in the laboratory 6 that enable “touch” a virtual world.
In further work, the results obtained with the proposed methodology will be compared to those provided by a vision-based human motion capture system that will be used as a reference attitude estimation system and embedded in robots to assist people and improve human performance in daily and task-related activities, focusing in particular on populations with special needs, including those convalescing from trauma, rehabilitating from cognitive and/or physical injury, aging in place or in managed care, and suffering from developmental or other social, cognitive, or physical disabilities. Robotics has the potential to both (1) serve as a tool for principled, analytical study of human behavior and (2) provide unique assistive technologies to improve quality of life.
Acknowledgments
The authors would like to thank to the National Polytechnic Institute (IPN)-SEPI ESIME UA and the SIP for the Projects 20130784 and 20130845, and for their support given in the process of validation of the methodology of estimation and for the use of theirs laboratory. This work is partially supported by VIEP-BUAP under Grant GUCJING13-I, and the UASLP for the Project PROMEP/103.5/12/7964.