# Field Navigation Using Fuzzy Elevation Maps Built with Local 3D Laser Scans
## 3 Obtaining Leveled 3D Point Clouds  
This section describes the acquisition of leveled 3D point clouds on Andabata while moving on irregular terrain. For this purpose, it is necessary to synchronize the motion of the vehicle with 2D  
Figure 4: Software scheme of Andabata.  
Figure 3: User interface of the Android app _Andabata_ on the tablet.  
scan acquisition. All in all, it represents a local SLAM scheme implemented at low speeds without loop closures.  
Let \(XYZ\) be the reference system associated with the robot, which is located at the geometric center of the rectangle defined by the contact points of the wheels with the ground. The \(X\)-axis coincides with the longitudinal axis of the vehicle pointing forwards, the \(Z\)-axis is normal to the support plane and points upwards and the \(Y\)-axis completes this orthogonal coordinate system pointing to the left (see Figure 5). The Cartesian coordinates for a leveled 3D point cloud are expressed in an \(X_{p}Y_{p}Z_{p}\) reference system whose origin coincides with that of the vehicle at the beginning of each scan. The \(X_{p}\) axis coincides with the projection of \(X\) on the horizontal plane, \(Y_{p}=Y\) and \(Z_{p}\) is defined with the same direction as gravity and opposite direction (see Figure 5).  
The Kinematic_model node is in charge of calculating the actual longitudinal and angular velocities of the vehicle (see Figure 4). For this purpose, it employs the encoder readings of the motors (obtained by the Motor_control node from the motor drivers) on an approximate 2D kinematic model of skid-steering [32].  
The Local_positioning node employs an unscented Kalman filter [33] to localize the vehicle with respect to \(X_{p}Y_{p}Z_{p}\) during the acquisition of the whole 3D scan. To this end, the node merges the roll and pitch angles, the yaw speed of the vehicle (provided by the smartphone and published by the Android_sensors_driver app), and the linear and angular velocities from the Kinematic_model node.  
For 3D laser scan acquisition, the Spinning_head node commands a constant rotation speed for the head and periodically receives (every 25 ms) the relative angle between the head and the base at the start of each 2D scan via the USB connection. In this way, this node can publish the transformation between the 2D laser sensor reference system and \(XYZ\). In addition, the 2D_laser_scanner node allows for configuring the Hokuyo sensor and publishes the 2D scans that the computer receives through the Ethernet connection.  
Finally, the Cloud_maker node publishes the 3D leveled point cloud by combining the successive 2D vertical scans (published by the 2D_laser_scanner node) with the relative transformations between the reference system of the 2D laser scanner and \(XYZ\) (provided by the Spinning_head node), and between \(XYZ\) and \(X_{p}Y_{p}Z_{p}\) (published by the Local_positioning node).  
With Andabata stopped, a 180\({}^{\circ}\) horizontal turn of the head of the 3D scanner is enough to capture all the surroundings of the robot, but, with the vehicle in motion, it is necessary to perform a full 360\({}^{\circ}\) turn. In this way, non-sampled areas around the robot are avoided and the blind zone above the robot is reduced (see Figure 6).  
Figure 5: Reference systems for the robot (\(XYZ\)) and for a leveled 3D scan (\(X_{p}Y_{p}Z_{p}\)).