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 Abstract This 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 He str 134 D 80797 Munich GERMANY fi rstname lastname tum 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 i 1 i 1 qi qi 1 qi 1 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 i i i i iXS m 0 0 qi i i i i 1 0 0 qi i i i i 1 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 i 1av i 1aS 1 i 1 i 1 i 1Xv i 1XS 1 i 1 i 1 i 1 i 1 i 1Xv i 1XS 1 4 where i 1Xv is the position of joint i i e the new virtual sensor which is denoted as Svin Fig 2 in joint frame i 1 i 1XS 1 is the position of the real sensor located on link i 1 and i 1aS 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 i 1 i 1 i 1 i 1and i 1av 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 Sm SmrSm SmbT 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 iai 1 i 1di 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 K 1 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 g V Fig 3 Block diagram of accelerometer model T t 25 C 25 Csin 1 2 t 11 Note that we do not assume to be able to measure tempera ture The bias is assumed as additive measurement noise in the dynamic model 2 C Extended Kalman fi lter The nonlinear output function of the dynamic system 2 requires a suitable estimator such as an extended Kalman fi lter EKF or unscented Kalman fi lter UKF We chose an EKF which linearizes the nonlinear model around the estimated state x t and applies the principles of a Kalman fi lter with the linearized system matrices 23 24 In order to properly tune KF and EKF the process noise covariance matrix Q together with the measurement noise covariance matrix R have to be known Measurement noise covariance matrix can be approximated according to sensor specifi cations provided by the manufacturers Since the es timator dynamics model is exact for all states except for q jerk has a large variance in the process noise covaria