1.

INTRODUCTION

From the large manipulator of industrial production line to the small household robot in the field of medical rehabilitation, joint angle measurement has always played an important role in the intelligent control field of robot. Only accurate and stable robot sensing information can accurately control the robot and its end effector¹. Oguntosin measured the angle of the exoskeleton rotating joint by using an incremental encoder². Dalia designed a photoelectric sensor measurement scheme that can be easily integrated into robot joints³. This method can be used for small robots, but the sensing range of joint angle is usually between 0° and 40°, which still has some limitations. The existing robot joint angle is usually measured directly by the angle sensor installed on the joint, which mainly includes Hall magnetic rotary encoder, optical encoder and so on.

Although these angle sensors have high precision, they have high requirements for installation mode which need occupy a large space, and are easily disturbed by environmental factors. Moreover, the joint size of some small robots currently cannot provide installation space for measuring devices, or cannot have built-in angle sensors. At this time, as a small and portable measuring tool, the IMU can realize repeatable and real-time monitoring without occupying the design space of the robot. Many scholars have used IMU to measure the robot angle^{4, 5}, but there are some typical problems. The first is that the direction of the sensor can not be consistent with the direction of the rotating axis of the motor during installation. The second is that the model can’t still maintain repeatability and accuracy in the face of different intensities of noise and vibration. The measurement method in this paper can bind the IMU to the robot in any posture, and determine the posture relationship between the IMU and the robotic arm in real time. It can more effectively avoid the installation error and the error caused by the sensor offset, so as to ensure the robustness and reliability of the robot in the whole measurement process.

This paper mainly uses the method of real-time calibration of IMU and human body by Seel⁶. The knee joint angles obtained by gyroscope and accelerometer are fused based on the results of calibration, so as to obtain real-time and stable joint angle data.

2.

SOLUTION OF THE POSE RELATIONSHIP BETWEEN THE IMU AND THE ROBOTIC ARM

The robot joint can be modelled as a hinge connection structure of a rigid body. As shown in Figure 1, two sensors are bound to the robotic arms on both sides in any installation attitude. At any time when the robot moves, the two sensors can collect N groups of angular velocity and acceleration synchronous data sets D(i) = {a₁(t_i), a₂(t_i), w₁(t_i), w₂ (t_i)}, i ∈ (1,N). Under this condition, the joint axis and joint position coordinates can be solved by establishing kinematic constraints⁶.

Figure 1.

Mathematical model diagram of robot joint based on IMU layout.

2.1

Identification of joint axis coordinates

As shown in Figure 1, s₁ and s₂ are two sensor coordinate systems respectively, j is the unit direction vector of the robot joint axis, and the unit direction vectors j₁ and j₂ are different expressions in the two IMU local coordinate systems. Therefore, j₁ and j₂ are parallel to j and expressed in spherical coordinate system as shown in equation (1), where θ and ϕ are the inclination angle and rotation angle in the local coordinate axis.

During the movement of the robot, the two mechanical arms rotate around the same joint axis and are closely combined without disjointness, the components of the vertical joint axis must be equal in size. In other words, w₁ and w₂ have the same projection length at each time in the geometric plane with the joint axis as the normal vector. Then for any time t there is equation (2).

Then the residual function of nonlinear optimization problem is defined:

2.2

Identification of sensor position

Similarly, the acceleration measured by the IMU sensor can be considered to be composed of the acceleration of the joint center and the acceleration generated by the rotation of the IMU sensor around the joint center^{7, 8}. Obviously, the magnitude of the acceleration at the joint center is the same in both IMU coordinate systems. So for any time t, there are equations (4) and (5).

is the acceleration generated by the rotation of the IMU sensor around the center of the joint, r_n is the displacement vector of IMU from the joint in its coordinate system, ẇ_n(t_i) is the derivative of the angular velocity measured by the gyroscope to time, which can be solved by the fourth-order Runge Kutta method. Finally, the residual function of the nonlinear optimization problem is defined according to the acceleration constraint formula:

Therefore, equations (3) and (6) are two nonlinear least squares problems, x = [θ₁,ϕ₁,θ₂,ϕ₂]^T is the object to be optimized, which can be solved by Gauss Newton.

2.3

Intelligent and accurate identification method of joint axis based on weighted constraints

There are four possible situations for joint axis symbol pairing: (j₁, j₂), (–j₁, j₂), (j₁,–j₂), (–j₁,–j₂). In other words, there may be a 180° difference in the expression of the joint axis direction vector in the two sensor coordinate systems, which will affect the measurement results. In this paper, the residual weight will be automatically adjusted based on the nonlinear weighting of angular velocity signal variance and acceleration norm difference^{8, 9}, which can automatically generate accurate joint axis estimation. When the velocity change is much less than 9.8 m/s², the acceleration measurement can approximately represent the gravity vector to obtain the acceleration constraint¹⁰. Then equation (3) of the least squares problem can be transformed into the following equations:

where x represents the unknown variable x = [θ₁,ϕ₁,θ₂,ϕ₂]^T to be solved. e_w(i,x) and e_a(i,x) are residual terms, respectively. is the maximum sample variance of w or a. The weights of the two residual terms are as follows: w_w(i) =σ_a/σ_w, w_a(k) = {1/[1+(||a₁(t_i)||–||a₂(t_i)||)²]}^1/2.

4.

JOINT ANGLE FUSION BASED ON ADAPTIVE KALMAN FILTER ALGORITHM

Since the accelerometer is easy to produce instantaneous vibration error in a short time, and the gyroscope will produce cumulative drift error in a long time. Therefore, this paper uses adaptive Kalman filter for attitude fusion. The flow chart of Kalman filter is shown in Figure 2.

Figure 2.

Flow chart of Kalman filtering.

Theoretically, only when the structural parameters and noise statistical characteristic parameters of the stochastic dynamic system are accurate and consistent, the standard Kalman filter can obtain the optimal estimation of the state. However, in practice, there are some errors that cause the filter divergence. In this paper, an adaptive algorithm of measurement noise variance matrix is used, which can estimate the noise parameters of the system online in real time through measurement while estimating the state. The measurement noise variance R_k is constructed by the method of exponential fading memory weighted average, as shown in equation (10).

Among the equation (11), initial value ζ₀=1, fading factor b = 0.98. At the same time, this paper adopts the method of setting thresholds to establish the upper limit condition R_max and the lower limit condition R_min to ensure that is positive definite, so that is always limited within [R_min,R_max] so it has better adaptive ability and reliability. The measurement noise established according to this idea is shown in equation (12), where .

5.

MEASUREMENT EXPERIMENT OF ROBOT JOINT ANGLE

In this experiment, the LPMS-B2 wireless inertial sensor of Guangzhou Arubi Company is selected, which has high dynamic accuracy. This experiment was based on the knee joint rehabilitation robot for research, which can realize various movements such as straight leg raising and belly bent legs, as shown in Figure 3.

Figure 3.

Knee rehabilitation robot experimental platform.

According to the calibration and angle estimation method proposed above, the joint angle measurement of the robot was carried out under two actions according to different placement methods, and the obtained joint angle curves were shown in Figures 4 and 5 respectively.

Figure 4.

Joint angle estimation of rehabilitation robot under bending leg flexion and extension.

Figure 5.

Joint angle estimation of rehabilitation robot under straight leg raising.

The four ways to place IMU on the robot are as follows: (a) Two sensors are attached to the front of the connecting rods respectively; (b) Sensor 1 is attached to the front of connecting rod 1, and sensor 2 is attached to the outer side of connecting rod 2; (c) Sensor 1 is attached to the outer side of connecting rod 1 and sensor 2 is attached to the front of connecting rod 2; (d) The two sensors are respectively attached to the outer side of the connecting rods.

It can be seen from the obtained angle curves that the drift error of the gyroscope increases with time, and the accelerometer vibrates obviously at the turning point of each rehabilitation action, but the angles after fusion are relatively stable. The experiment shows that the measurement method has good robustness under different placement poses.

6.

CONCLUSION

In this paper, an IMU-based robot joint angle measurement method is verified through the IMU and robotic arm calibration, sensor fusion and other processes. In order to simplify the experiment, a two-dimensional rigid body motion model is used, and the joint angle is only measured in one degree of freedom. The joint angle curve obtained on the robot verifies its high measurement accuracy and stability, and the measurement results have a high correlation, which shows that the system has excellent linear response ability. In particular, the proposed adaptive filtering algorithm can effectively combine the advantages of gyroscope and accelerometer.

In the next step, the correction of the experimental model and the calibration of motion error will be perfected and the joint angle measurement experiment under different temperature environment will be considered in the future research, so as to achieve better function. Compared with the traditional coded angle measurement method, the method used in this paper has advantages in installation flexibility, and reduces the burden for the joint structure design of the manipulator. Theoretically, the system can be used to make a detachable angle measuring device, which can be widely used in the field of industrial production. At the same time, the sensing and detection system can be integrated into the robot control system to realize effective closed-loop monitoring and control, which lays a solid foundation for the design of more intelligent rehabilitation robot control system.