For these experiments, data was collected from the sensors and robotic manipulator during motion, in real time. The collected data was then calibrated offline to align measurement frames and time sequences. The calibrated data was pre-processed to extract image features, discard outlier measurements and perform data format conver- sion.
This section describes additional pre-processing of measurements, converting data from raw sensor values into quantities used by the observers described later in this thesis. Data was read from log files into MATLAB data structures. Robot pose mea- surements were converted from quaternion orientation and vector positions measure- ments into homogeneous transforms. Image features were labelled and extracted from camera measurements, and from feature sets the camera pose was estimated. IMU measurements were pre-filtered to discard extreme values indicative of erroneous data packets.
3.7.1
Robot Data Pre-Processing
Pose data logged from the robot controller was stored as a quaternion giving the tooltip orientation, and a three-vector giving the tooltip position. The raw data for each mea- surement was read into MATLAB and converted into a homogeneous transform 4×4 matrix.
3.7.2
IMU Data Pre-Processing
IMU data was pre-processed by reading it into a MATLAB data structure and filtering it to remove erroneous measurements and correct for scale and time-constant bias er- rors in the gyrometer measurements. Raw data from the IMU was contained in packets consisting of an accelerometer, a gyroscope and a magnetometer measurement, each a three-vector.
3.7.2.1 Erroneous Measurement Removal
Erroneous measurement were detected by calculating the norm of the vectorial mea- surement on each signal and rejecting those above a threshold. Causes of erroneous packets can include electrical faults and interference in the serial channel and vibration in the IMU mounting.
The three inertial data signals, the accelerometer, gyrometer and magnetomers were tested and based on a fault in any signal, the entire packet containing that mea- surement was discarded. Figures 3.8, 3.9 and 3.10 depict the raw acceleration mea- surements, a histogram of the acceleration measurement norms and final measurment sequence after discarded erroneous packets. Similar patterns are observered in the gyrometer and magnetomer measurement channels.
3.7.2.2 Gyrometer Scale and Constant-Bias Correction
Initial comparision of the Gyrometer measurements to theoretically predicted mea- surements indicated the presence of large scale error in the measurements, together with noticable constant biases. It was decided to correct the scale error in pre-processing and, as the process of scale correction required bias estimation, correct for large time- constant biases.
A windowed portion of the the actual and expected gyrometer measurements were low pass filtered and estimates of the scale factor and constant bias calculated using a least squares estiamte. Modeling the actual measurement process asyk =axk+b, whereyk is the actual measurement, xk is the expected measurement, ais a constant scale factor and b is constant bias, define ˆyk=axˆ k+bˆ were the hatted symbols are estimates of their unadored kin. These equations may be represented vectorially as
Y =X A where A= a b ! , X = x1 1 .. . ... xn 1 , Y = y1 .. . yn (3.2)
and analogously ˆY =XA. Define the cost functionˆ φ=∑n1(yˆk−yk)2= (Yˆ−Y)>(Yˆ−
Y). Setting dφ
dAˆ =0, one has ˆA= (X>X)−1X>Y.
Using this estimate, of the scale factor and bias, the gyrometer measurements were corrected as part of the offline pre-processing. Output of this process is depicted in Figure 3.11. Note that this corrects for a time-constant estimate of the bias in the
0 50 100 150 200 250 −10 −5 0 5 10 X
Raw IMU measurements, including erroneous measurements
0 50 100 150 200 250 −10 −5 0 5 10 Y 0 50 100 150 200 250 −10 −5 0 5 10 Z t
Figure 3.8: Raw acceleration measurements as received from IMU. Note the apparent superposition of high magnitude noise on the underlying signal with low magnitude gaussian noise. Similar patterns are observered in gyrometer and magnetometer signal channels.
gyrometer signal, but leaves a residual time-varying bias component which must be estimated online.
3.7.3
Camera Data Pre-Processing
Camera image data was pre-processed to identify, label and extract image features and calculate a pose estimate for the camera at each frame.
Image features were labelled, their coordinates extracted and correspondences be- tween images specified using an assisted manual image feature identification program in the MATLAB environment. For the fist image in a sequence, the user was prompted to click on four image features, at the vertices of the squares described in Section 3.4,
−2 0 2 4 6 8 10 12 0 2000 4000 6000 8000 10000 12000 14000 Number of measurements
Acceleration measurement norm Histogram of Acceleration measurement norms
−2 0 2 4 6 8 10 12 0 10 20 30 40 50 60 70 80 90 100 Number of measurements
Acceleration measurement norm Histogram of Acceleration measurement norms (detail)
Figure 3.9: Histogram of norm of acceleration measurement vectors in raw IMU mea- surements. 0 50 100 150 200 250 −1 −0.5 0 0.5 X
IMU measurements with erroneous samples discarded
0 50 100 150 200 250 −0.5 0 0.5 Y 0 50 100 150 200 250 −1.1 −1 −0.9 −0.8 −0.7 Z t
Figure 3.10: Acceleration measurements after discarding erroneous measurements
0 50 100 150 200 250 −0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15
Uncorrected Angular Velocity Data
Omega (true) Omega (IMU) Error 0 50 100 150 200 250 −0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15
Scale and Bias Corrected Angular Velocity Data
Omega (true) Omega (IMU) Error
Figure 3.11: Uncorrected and Corrected low-pass filtered gyrometer measurements.
age and image feature edges smoothed using dilation and erosion, then the image was segmented. From the coordinates of the mouse clicks, black/white segment numbers for each image feature were identified. Final image coordinates for each image fea- ture were determined by calculating the center of mass of their segment. Following a successfully labelled image, an attempt was made to process the next image automati- cally, taking the image coordinates of the image features in the last frame as the mouse click locations. If tracking was lost, indicated by multiple features having the same black/white segment or the wrong colour segment, processing was reinitialised using the frame on which tracking was lost as the first frame in the sequence, prompting the user to manually identify image features again.
This simple identification and tracking scheme proved robust at tracking the im- age features and excellent at detecting lost tracking for the experiments trajectories used; in particular for moderate camera movement between frames. It seldom pro- duced false tracking positives, for example where image features tracks switched to
non-corresponding black/white segments. This is partially due to the design of the vi- sual target, in Figure 3.6, where each feature is spatially distinct from its neighbours, and lighting conditions to avoid shadows. In the rare case of false tracking positives, tracking was manually reinitialised.
The image feature coordinates extracted from this process were two dimension coordinates in the image plane. From these, image feature coordinates on the unit sphere were calculated using the intrinsic camera parameters estimated in Section 3.6.1. Camera pose was estimated for each frame from its set of image features us- ing thecompute extrinsicfunction of the MATLAB camera calibration toolbox (Bouguet 2008).