IROS2019国际学术会议论文集1959

Joint Velocity and Acceleration Estimation in Serial Chain Rigid Body and Flexible Joint Manipulators Seyed Ali Baradaran Birjandi, Johannes K uhn and Sami Haddadin AbstractThis paper deals with the problem of accurately computing and estimating joint velocity and acceleration in robotic manipulators. Generally, it is well known that numerical differentiation of noisy position signals even with signifi cant fi l- tering is no viable solution. This is especially true for computing joint acceleration. Specifi cally, our solution to this problem fuses joint position measurement with link accelerometers, which are affordable and easy to install. Since the sensor readings are affected by noise, drift and bias, suitable data fusion and fi ltering methods are proposed for improving the estimation for practical use. Simulation results based on a realistic dynamics model of a 7-DoF robot including various parasitic effects and experimental results with a 7-DoF robot demonstrate the effectiveness of our approach. This method would have multiple use, e.g., in monitoring external joint torques and handle possibly unforeseen collisions. Furthermore, other applications such as load identifi cation and compensation as well as state feedback linearization for fl exible joint robots could fi nally become possible also practical. I. INTRODUCTION Various areas of rigid body and fl exible joint manipulators such as motion control 1, 2, dynamic model identifi cation 3, robust control 4, friction compensation 5, collision detection 6 and safety in general 7, 8 would benefi t from highly accurate joint velocity and acceleration signals which, as of today is not available in serial chain articulated robots. The conventional approach to estimate joint velocity is to use tachometers or numerically differentiated position encoder signals. This process is well-known to be very sensitive against noise and/or quantization effects 9. Optical en- coders or resolvers and tachometers on the one hand are not only rather expensive but also noisy 10, 11. For instance, numerical differentiation in practice introduces noise and/or an intrinsic one-step delay in a digital implementation for obtaining joint acceleration 12. The focus of this paper is to resolve this limitation in robot control and state estimation. For this, we estimate joint veloc- ity and acceleration by incorporating accelerometers installed on the corresponding links together with the commonly available joint position measurement, see Fig. 1. Related work can be found in 13, where the body state of a Hexapod (rigid body) is estimated using leg pose and inertial sensors. However, the authors assumed that angular acceleration and velocity of a rigid body (separate links in manipulators) are unknown. Thus, one requires at least four accelerometers on each link to compute joint velocity and acceleration with The authors are with the Chair of Robotics Science and Systems Intel- ligence, Munich School of Robotics and Machine Intelligence, Technical University Munich (TUM), Hestr. 134, D-80797 Munich, GERMANY, fi rstname.lastnametum.de Fig. 1: Acceleration and velocity estimation of robot joint i the proposed method. In a similar work, Jassemi-Zargani et al. 14 used global geometrical information, i.e. the Jacobian of the manipulator, to estimate joint variables. This, however, signifi cantly increases computational complexity and also prohibits decentralized computations on joint level. On the other hand, sensor installation misalignment may also pose a signifi cant problem. Specifi cally, even minor installation errors lead to estimator performance degradation, which intensifi es with time 15. Parsa et al. 16 employed an array of redundant triaxial accelerometers in order to compensate sensor installation errors, making the setup also computationally more challenging. The installation errors could indeed be eliminated by suitable sensor calibration before running the manipulator. Moreover, if the installation error dynamics is known (which is a reasonable assumption), it may be fi ltered out from the sensor output. However, the installation error of accelerometers alone hardly justifi es equipping the manipulator with a larger number of sensors than required, although that might improve accuracy. Further similar work can be found in 17, where Munoz-Barron et al. propose an approach for feedback control of open-chain rigid body manipulators by measuring and fusing the information from optical encoders, accelerometers and gyroscopes. To the best of the authors knowledge, this paper is the fi rst practical estimation and implementation of joint velocity and acceleration based on joint position and link acceleration sensing only. The characteristics of this work is simplicity of computations allowing to perform calculations on-board at high speed, wide estimation bandwidth thanks 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE7497 to sensor fusion of different sensing modalities, namely joint position and Cartesian acceleration sensors and reduction in the number of required acceleration sensors in comparison to existing approaches. The results may e.g. be used for feedback acceleration control 18 or collision detection 19. In 20 estimated joint velocity and acceleration of an intrin- sically elastic robot based on force sensing measurements are employed using nonlinear observing method for blind (no vision) dribbling. In contrast to existing approaches in which the link angular variables are considered to be entirely unknown, we simplify the problem by separating the angular velocity and acceleration into a known and unknown part, with the latter depending on information from other joints. Thus, our approach requires only one accelerometer per link for fi xed-base robotic manipulators. The specifi c infl uence of the number of accelerometers and the optimality of sensors placement on the estimation procedure and achievable accu- racy will be analyzed in future works. The remainder of the paper is organized as follows (see also Fig. 1). Section II introduces the considered research problem. Methods for fusing position and acceleration sen- sors, noise fi ltering and joint variables estimation are gath- ered in Section III. For experimental validations, a realistic dynamics model of a 7-DoF fl exible joint manipulator is simulated and the results for different analysis are reported in Section IV. Similar experiments are performed for a 7-DoF fl exible joint robot in Section V. Finally, the paper concludes in Section VI. II. PROBLEMSTATEMENT In order to be able to estimate the joint acceleration, let us fi rst consider an arbitrary link i of an articulated manipulator whose link side joint velocity qi R and acceleration qi R in joint i are unknown, see Fig. 2. The angular velocity and acceleration of link i are denoted by i R3 and i R3. Furthermore, a triaxial accelerometer mounted on i ia i i1 i1 qi qi+1 qi1 S2 Sv S1 Fig. 2: Conventions for robot arm kinematic quantities as well as schematic of 2 real and 1 virtual accelerometer mounted on the arm link i measures Cartesian link acceleration ia R3 in the accelerometer frame. The accelerometer frame is determined according to its sensing axis. Since the accelerometer loca- tion is fi xed, the orientation of the sensor frame remains fi xed with respect to the orientation of frame i, however, with a different origin. Due to noise and bias, the accelerometer signal needs to be fused with other sensors. The standard measured quantity of an n DoF robot is the joint position q Rn. Time derivatives of the measured joint position alone is also erroneous due to sensor quantization and effective noise. In this paper we develop a framework for estimating joint velocity and acceleration based on fusing joint position mea- surement and accelerometer placed on the link to improve the estimation accuracy and bandwidth. III. SENSORDATAFUSION ANDFILTERING In this section, fusing acceleration sensing with joint po- sition measurement is introduced. Furthermore, considering the fi ltering of noise as well as estimating unknown sensor bias (or slow drifts), suitable fi ltering is evaluated based on two different kinematic models. Note that the subsequently mentioned fi lters could be one of various schemes such as Kalman or extended Kalman fi lters. However, the exact fi l- tering method is not of particular interest but the underlying model. A. Linear fi ltering without acceleration measurement In the linear fi ltering approach, joint position, velocity, acceleration and jerk are the states of the dynamic model of the fi lter and it has the form d dt qi qi qi . qi = 0100 0010 0001 0000 qi qi qi . qi + w, y = qi + v. (1) Here, . qi R denotes jerk with w R4and v R being process and measurement noise with known covariance matrices, respectively. y is the output function. The exact link motion dynamics is assumed to be unknown. Therefore, the general motion dynamics based on the states time derivatives is used in the fi lter. This simplifi es and generalizes the fi lter for any given trajectory. Moreover, given that the dynamic model of the joint acceleration in particular is not at hand, we insert its time derivative with zero dynamics into the Kalman fi lter dynamic model for reducing process noise. Constant jerk in general is an inherently false assumption. However, this choice introduces less process noise than a constant acceleration assumption and consequently improves acceleration estimation. The estimation is done based on qi measurement only. B. Nonlinear fi ltering The nonlinear estimator uses the Cartesian acceleration and joint position measurements for the fi lter error correc- tion. This leads to the nonlinear dynamic model d dt qi qi qi . qi = 0100 0010 0001 0000 qi qi qi . qi + w, y = ? qi ia(qi, qi, qi) ? + v.(2) 7498 The measurement error v R4in this model includes both accelerometer white Gaussian noise and bias. Note that in closed loop systems where the control input is available (such as in the feedback linearization control method), the infor- mation of the desired trajectory could replace the constant jerk of dynamics model in the estimator. This might increase estimation accuracy in tracking control applications. The nonlinearity in (2) originates from the output function in which ia(qi, qi, qi) is obtained nonlinearly. The Cartesian acceleration measurement iaS m R3from the m-th sensor Sminstalled on link i adjacent and successor to link i 1 can be decomposed into iaS m = ial +i i iXS m+ ii ?i i iXS m ? 0 0 qi =iiii1, 0 0 qi =i ii i1. (3) The position of the sensor with respect to joint frame i is denoted as iXS m R3 and considered to be identifi ed by calibration (see Sec. III-C). The offset ial is the lin- ear acceleration caused by the joint frame translation. For computing ial (including gravity) of the i-th link frame, a virtual acceleration sensor (superscript v) located on joint i is introduced. Its output can be obtained with the help of data from the previous link as i1av = i1aS 1+ i1 i1 ?i1Xv i1XS 1 ?+ i1i1 ? i1i1 ?i1Xv i1XS 1 ?, (4) where, i1Xv is the position of joint i (i.e., the new virtual sensor which is denoted as Svin Fig. 2) in joint frame i1, i1XS 1 is the position of the real sensor located on link i 1 and i1aS 1 is its output. Given iav is not affected by joint i rotation, it will be the linear acceleration of the i-th link after it is transformed into joint frame i, i.e. ial = iav. iaS m in (3) is determined recursively from the fi rst sensor installed on the fi rst link to the last sensor located at the end-effector. One has to make sure that the correct pieces of information are sent between two adjacent links, i.e. i1i1,i1 i1and i1av. Since the measurement function is nonlinear, a suitable technique such as an Extended Kalman Filter is used for data fusion and fi ltering. In order to implement an EKF, (2) needs to be discretized and the exact discrete-time representation of the dynamic model (which is linear) is used. Since y in (2) is an algebraic equation, discretization does not affect it. C. Sensor misplacement error estimation and calibration Given the algorithm is dependent on the sensor posi- tion, misplacement may greatly affect the estimation results. Therefore, a calibration procedure is explained subsequently. Using this algorithm position and orientation errors as well as the sensor bias can be detected and compensated. Even though accelerometer bias is typically a function of time, initial calibration helps for later tackling it more effectively. An important assumption we make is that the sensor is fi rmly attached to the link, i.e, the transformation matrix which transforms the sensor frame to its corresponding joint frame (iTSm) is constant. Assuming this transformation matrix in accordance to Denavit-Hartenberg (DH) convention and adding three elements of bias, seven parameters need to be estimated in total to properly calibrate each accelerometer. Since these parameters are assumed to have zero dynamics, we only discuss the measurement function of the estimator, which is given by waS m = JSm(dSm,rSm,Sm) qi + JSm(dSm,rSm,Sm) qi+ gw iaS m = RSm i (Sm)Ri w(qi) waS m+ b, (5) where waS m denotes the Cartesian acceleration of the sensor in world frame w. The DH parameters are denoted by dSm,Sm,rSmand Sm, and JSm R3denotes the sensor Jacobian, gw R3the gravity vector expressed in w, iaS m denotes the sensor output in its frame, RSm i (Sm)Ri w(qi) denote the rotation from w to sensor frame, and b R3 the sensor bias. Given that the trajectories are assumed to be known during the calibration process, qi, qiand qiare measured by highly accurate external tracking system in (5). In order to deduct the effect of gravity, we need a reference frame (i.e. world frame) in which the gravity vector is independent from the robot state. Therefore, the Cartesian acceleration is initially computed in the world frame. In sum- mary the sensor pose and bias estimator can be formulated as x = ?d SmSmrSmSmbT ?T , x = 0 y = iaS m( x). (6) One joint at a time must be excited while the accelerometers mounted on the corresponding link are calibrated. In order to properly estimate the bias, it is recommended that a sine wave with periods of stagnation is applied to each joint. Once the parameters are estimated, iXS m in (3) according to Denavit-Hartenberg convention because iXS m = rSm dSmsin(Sm) dSmcos(Sm) . (7) Furthermore, the measurement function in (2) can be rewrit- ten as y = ? qi ia(qi, qi, qi) + b ? + v.(8) As mentioned earlier, one of the advantages of our approach is the independence of global robot geometrical information. Therefore, frame w in this section is not necessarily located in the robot base. In fact, it may be placed on the currently actuated joint. Thus, JSmand JSmof (5) are independent from the robot geometry and state. 7499 IV. SIMULATIONRESULTS The robot dynamics considered in the subsequent simula- tion is a 7-DoF fl exible joint robot dynamics model equipped with all related parasitic effects known from robot design. The used sensors are as follows. A Matlab/SIMULINK model of this robot is used for the evaluation of the developed schemes. The Denavit-Hartenberg parameters are listed in Table I. TABLE I: Denavit-Hartenberg parameters of the 7 DoF manipulator Joint iai1i1di(m)i 1000.31q1 20/20q2 30/20.4q3 40/20q4 50/20.39q5 60/20q6 70/20q7 A. Joint position sensor model The joint position is measured and quantized by a 16- bit encoder. Given the joint torque Jand motor side position are measured, the link position can be modeled as 21 q = K1 J J,(9) in which q is the estimated link position measurement and KJthe estimated joint stiffness. The measurement of q is assumed to be polluted by normally distributed random noise with amplitude of three bits. Furthermore, quantization, fi ltering (fi rst-order low-pass fi lter with cutoff frequency fc= 300 Hz) and saturation are modeled. The parameters of the used encoder are: number of bits = 16, number of LSBs affected by noise = 3 and cutoff frequency of the fi rst-order low-pass fi lter = 300 Hz. B. Accelerometer model In the simulation the noise- and bias-free Cartesian accel- eration signals are computed from JSm(q, q) q + JSm(q) q,(10) where JSmdenotes the geometric Jacobian of the sensor placement. The parasitic effects are modeled according to the chosen exemplary accelerometer ADXL326 from Analog Devices 22. The sensor is a 3-axis accelerometer, which outputs analog acceleration-proportional voltage. The char- acteristics of the sensor for all three axes and the probability distribution of 0 g offset versus temperature and sensitivity change due to the temperature parameters can be found in the datasheet 22. The sensor built-in second-order Low- Pass Filter (LPF) is also simulated. This fi lter truncates the Additive White Gaussian Noise (AWGN) and its cutoff frequency can be adjusted manually. Fig. 3 depicts the block diagram of the accelerometer model. Since the bias characteristics of the sensor (0 g offset versus temperature and sensitivity change due to the temper- ature) are functions of temperature, the simulation contains the time-varying temperature profi le 0g Bias Level AWGN JSm q + JSm q Kinematics q, q, q LPF Cut-off frequency 300 Hz 1 mg/CT(t) 0.057 mV/g 0.01%/C 0g Voltage, 1.5 V a gV Fig. 3: Block diagram of accelerometer model T(t) = 25C + 25Csin(1.2t).(11) Note that we do not assume to be able to m