3-D Lidar for mobile machinery
Autonomous mobile work machines require accurate maps of their surroundings in order to be able to perform the required tasks efficiently and especially safely. The machines need first of all rough information of their position at the work site, where an accuracy of a few meters is often acceptable. This can be provided to the machine with GPS-based navigation. However, in addition they need high-resolution information—accurate to a few centimeters—of their position in relation to the nearby objects and environment. As the machines are constantly moving, this information needs to be constantly updated.
Some commercial solutions of 3-D Lidars can be found on the market, for example from manufacturers such as Sick  and Velodyne , but in many cases these are quite expensive, especially for outdoor applications. Therefore, there is a need for a robust low cost 3-D Lidar, especially for use in research. This paper presents a solution for a low cost 3-D Lidar based on a 2-D laser scanner, an electric motor drive rotating the 2-D scanner, and data fusion with the navigation system of the mobile machine. The 2-D laser scanner provides range and intensity data from the measured plane. The controller of the electric motor drive provides the rotation angle and rotation speed of the laser scanner.
By combining these measurement data with the navigation data one can create point clouds that can be used for sensing and mapping the environment of an autonomous mobile machine. In this paper, the mechanics and control system structure of the servo are presented in detail to make building similar types of systems easier for the interested reader. The developed hardware described in this paper was made for the laser mapping of the autonomous mobile machine used in the GIM project . The rotating laser scanner scans the fore field of the machine. The laser map is used among other types of maps for path planning and obstacle avoidance purposes.
Control system of an autonomous mobile machine
The work machine, for which the mapping system was developed, is a modified version of a multi-purpose wheel loader. The frame of the machine is original, but the control system, electronics, and hydraulics have been changed to enable researching autonomous operations. The control system architecture of this autonomous machine is illustrated in Figure 1.
The hardware devices are located physically in two different locations. Visualization and operator computers are off-board computers and those are connected through a network switch and over WLAN to an on-board network switch. On-board computers and peripheral devices are also connected to the on-board switch. The on-board computers and peripheral devices follow the device architecture illustrated in Figure 1.
The low-level control device architecture consists of a control of the actuators, such as hydrostatic drive pump, diesel engine, and hydraulic valves of the machine. It also takes care of the data logging of the inertial measurement unit (IMU), central joint resolver, pressure, and some other sensors. The low-level control is based on six Epec embedded vehicle computers, which communicate with the middle level control through four CAN networks. The implementation of CAN followed the CANopen standardization, which enables fluent CAN network management and consistency handling.
The middle level control, including navigation, path planning, and network interfaces with real-time kinematic global navigation satellite system (RTK-GNSS) was implemented on an industrial embedded PC using Matlab/xPC Target as an operating system. The control of the servo—including the electric motor drive and the encoder measurement—were also implemented in this level via CAN. The high-level control of device architecture is also implemented on an industrial embedded PC. This PC communicates with the middle level using the UDP protocol. This protocol is also used in data transfer from the 2D laser scanner to the middle level control to ensure a real-time data transfer of the laser scanner data.
Electric motor drives and 2-D laser scanners
In general, two different types of DC motor exist: Brushed direct current (DC) motors and brushless direct current (BLDC) motors are the most typical motors. BLDC motors are typically driven by three phase conductors and phase voltages are generated by the braking voltage of the intermediate circuit. In accurate position and speed electric motor drives, BLDC motors are more common.  Thus, a BLDC motor was also used in this case. A ceramic planetary gear was used for the reduction of the output rotation speed and to increase the output torque.
The accurate and not (or in practice minimum and constantly) delayed measurement of the rotation angle is essential to later enable data fusion with laser scanner range data for creating a 3D point cloud data. This is more important than for example an accuracy of the position or speed control of the servo because in these cases a small error or delay is not reflected to in the 3-D point cloud calculation.
DC motors typically use a built-in brushed commutator for commutation but the selected BLDC motor requires an external sensor to sense the angle and speed of the rotor for commutation. The commutation type depends on the sensor type and it can be sensor-less commutation, six-step commutation or sinusoidal commutation. Sensor-less commutation doesn’t use sensors at all, six-step commutation uses Hall sensors, and sine commutation uses for example incremental encoders for commutation. Sine commutation is the most recommended commutation type if the application requires a constant torque generation over the whole rotation speed range .
Thus, an external incremental encoder was selected for measuring the speed of the electric motor for speed control purposes. The measurement of the incremental encoder is a relative measurement, referenced to a certain reference rotation angle of rotor. It can’t be directly used as a rotation angle measurement because every time the sensor is started it has to be calibrated, i.e. by finding the zero position.
Therefore, an external absolute encoder was selected for measuring the rotation angle despite the fact that absolute encoders are typically more expensive than incremental encoders because the internal structure of absolute encoders is more complex. Because data fusion with laser scanner range data requires the accurate measurement of the laser scanner rotation angle, the rotation angle measurement is made directly from the load side. Thus, inaccuracies resulting from the backlash of the planetary gear can be avoided.
Control system of the electric motor
In this paper, the control system of the electric motor controller is based on two control loops as illustrated in Figure 2. Two control loops were used because the system includes a planetary gear that brings backlashes to the system under control. Furthermore, backlashes cause delays in control systems and it might have an effect on the system stability. The planetary gear is needed because it is hard to precisely estimate the torque that the load requires. The purpose of an auxiliary control loop is to stabilize, define damping and the dynamic behavior of the system. The sensor in the auxiliary control loop is an incremental encoder that is located at the backend of the electric motor. The primary control loop uses an absolute encoder as a feedback sensor and controls the load angle of the rotation.
All controllers in the control system are implemented to the electric motor controller as discrete time controllers. The current regulator is a PI controller with 10-kHz sampling time; the speed controller is implemented as a PI controller with speed and acceleration feed forward. A position controller is implemented as a PID controller with speed and acceleration feed forward.
The sampling times for both controllers are 1 kHz. In the control system, the speed feed forward can compensate speed-dependent friction that is caused by bearing among other things. The acceleration feed forward provides more current in cases when one needs a high acceleration or the load inertia is high. The electric motor controller is delivered with ready-made software that can be used for controller tuning.
2-D laser scanners
The purpose of the electric motor drive is to continuously rotate a 2-D laser scanner. The scanner type—a Sick LMS111—was predefined for this hardware by the end user. Table 1 presents the most relevant properties of the scanner. The laser scanner and its final installation position on the roof of the autonomous machine are presented in Figure 3. The measurements in Figure 6 illustrate how the position (left) and angular speed (right) behave over one movement cycle. One can clearly see that profiles are quite smooth over time and only a small angular speed ripple exists. The operating principle of the 2-D laser scanner is based on the time-of-flight (TOF). The 2-D laser scanner gives range and intensity values as measurement values from the measured plane. Measured range values are relative to the rotating mirror of the 2-D laser scanner and for every measured point one can also get the intensity value .
Hardware and software of the 3D laser scanner
The system level requirements based on simulations were defined as follows: movement range 100°, field of view 140° at the whole movement range, maximum rotation speed 40°/s. The total mass of the load of the electric motor was about 2.3 kg and the calculation of the inertia of the load was based on this mass, the dimensions of the laser scanner, and assuming a homogeneous mass distribution. Based on this data, a Maxon EC-max 30 was chosen as an electric motor drive and planetary gear. The electric motor has the parameters shown in Table 2.
The electric motor drive contains a planetary gear, so the electric motor controller must support two control loops. In the rotation speed loop, an incremental encoder with 1000 counts per turn (CPT) and an index channel was used. In the position control loop, a single-turn absolute encoder with 16-bit resolution was selected. The purpose of shunt regulators is to prevent current flowing back to power source. Returning current can cause the power source output voltage to rise. Table 3 presents all components that were needed for the implementation of the electric motor drive.
The mechanics of the hardware consist of two different units. The control unit contains all components that are closely related to the electric motor controller and the rotation unit contains all components that are closely related to the electric motor and sensors. Between these two units exists only wiring for the electric motor and sensors. The case of the electric motor controller did not offer enough protection for a demanding environment; accordingly the case was improved with an additional aluminum case. This aluminum case is easy to move and can be installed for any mobile machine. This control unit with panel connectors is illustrated in Figure 4.
The 2-D laser scanner is supported from both sides with slide bearings. These slide bearings are marked yellow in Figure 4. Slide bearings support the mechanical structure so that an axial movement of the 2-D laser scanner is prohibited. Another purpose of the slide bearings is to tolerate a radial load. The slide bearings’ material is a brass alloy and the axle material against this brass alloy is Hydax 15 steel.
The mechanical structure and manufacturing technique might cause axial misalignment, so components that fit the axial misalignment are needed. Mechanical coupling on both side of the 2-D laser scanner makes this axial misalignment fitting, and mechanical coupling forward rotational motion and torque generated by the electric motor and planetary gear. The mechanical coupling fits the axis of the planetary gear and the axis of the mounting patch, seen on the left side of Figure 4. Furthermore, the other mechanical coupling fits the axis of the mounting patch and the axis of the absolute encoder on the right side.
The control system of the mobile machine runs on middle level control. The electric motor drive and 2-D laser scanner are controlled by a smaller control unit seen in Figure 4, which was integrated in an existing control system. The execution of the developed software follows the flow chart as illustrated in Figure 5. In the initialization phase, the electric motor controller and 2-D laser scanner are parameterized, devices are started, and the whole system goes into the waiting state. In the waiting state, the hardware waits for a command from the control system.
In the initialization phase of the 2-D laser scanner, the FOV and the angle resolution are set. During the initialization phase of the electric motor controller, the state machines are set to the right state and the periodic SYNC is set to CAN. In addition, the software is set to receive the rotation angle and speed data from the electric motor controller. Heartbeat receiving was also implemented.
The control system of the hardware implements two operating modes. "Drive to position" drives the 2-D laser scanner to a certain angle according to the given parameters and "Sweep between angles" rotates the 2-D laser scanner between certain angles with the given parameters. Both operating modes use the electric motor controller’s internal operating mode "Profile Position Mode" . Figure 6 presents the basic verification measurements of the electric motor drive.
The implementation of networks on the mobile machine is based on using CAN and Ethernet networks. The developed hardware uses both networks to transfer measurement data. Thus, the measurement data must be synchronized to the time domain in some way. One method for time synchronization is time stamping. Various devices can add time stamps to measurement data. For example, the 2-D laser scanner and the electric motor controller can add time stamps to measured data when devices send measured data to the control system. Based on these time stamps, the measurement data can be time synchronized. This requires that the clocks of both devices are synchronized frequently enough to prevent time drifting.
Some devices don’t provide time stamps for their measurement data or they are not usable for some reason. In this case, one must arrange time synchronization some other way. Thus, a deeper understanding of the behavior of networks and protocols is needed. Time stamping can be arranged in such a way that the receiver adds a time stamp to the measurement data. The receiver in this context is the middle level control.
This time of arrival (TOA) is useful when there is not much of a data transfer lag and a small time synchronization error is allowed. With this hardware, time stamping is done in the following way: the 2-D laser scanner sends the measurement data with a constant time interval, which is 25 Hz or 50 Hz. The measurement data is transferred from the 2-D laser scanner to the middle level control via Ethernet. The middle level control then sends a SYNC request to the CAN network. The electric motor controller responds to this SYNC request with data that contains the rotation angle and the rotation speed.
According to the datasheet, the output of the 2-D laser scanner is a real time output . Thus, it is assumed to work in such a way that the 2-D laser scanner measures the range data and sends the measured values to the middle level control through UDP with a low network latency right after measurement. The latency time in the data transfer in Ethernet networks is assumed to be negligible. We estimate that this latency time is less than one millisecond.
If the UDP packet sent by the 2-D laser scanner arrives at the middle level control at time instant txPC, then the worst case scenario is that the SYNC request is sent to the CAN network from the middle level control at the time instant txPC + 1 ms. The electric motor controller responds to the SYNC request quite fast. Based on CAN analyzer measurement, the time that passes between the SYNC request and the response from the electric motor controller is less than 500 µs.
In this context, we assume that the electric motor controller works as specified in the CANopen standard, which means the reaction to SYNC request comes within this measured time: sensors are read, the data is packed to a PDO, and the PDO is sent back to the middle level control. If the SYNC request is sent at the time instant tSYNC, then the response is available during the time instant tSYNC + 1 ms. When one takes all synchronization error sources into account, the total synchronization error is around 2 ms and it is caused by data transfer delay, program execution step size, and reading sensor to middle level control.
The internal operation of the 2-D laser scanner also affects the synchronization error. The internal rotating mirror of the 2-D laser scanner rotates with 25 Hz or 50 Hz so one cycle takes 40 ms or 20 ms. The UDP packet that contains the measurement data is sent at the time instance tS. This time instance takes place when the rotating mirror is in an area where the 2-D laser scanner does not measure. This region is marked blue in Figure 7. One can’t know this time instance exactly. The last measured point is measured at the time instance tM and this time instance differs fundamentally from the time instance tS. The time difference between these two time instances is around 5 ms to 10 ms if the measurement frequency is 50 Hz and FOV is 140 degrees.
Figure 7 illustrates the region in red that the 2-D laser scanner uses in this application. The time instance tM illustrated in Figure 7 is the place where the last measurement is done. When one takes all above-mentioned error sources into account, a maximum error of 15 ms time synchronization can exist between the last measured point and the rotation angle of the 2-D laser scanner.
The time difference between the measured range values and the rotation angle causes a time synchronization problem that affects the laser map quality because the time synchronization problem causes a motion synchronization problem. In this context, a motion synchronization problem means that there is an error between the position of the rotating mirror and the rotation angle of the servo of the 2-D laser scanner. Thus, the measured range values are not where the measured rotation angle expresses them to be. This error is related to the angular speed of the servo: a bigger angular speed means a larger error.
Operating with this hardware requires a time synchronization of the frame transformation. The coordination is needed between the rotating mirror of the 2-D laser scanner and the axle of the electric motor drive. The related frame transformations are calculated in , where the motion synchronization is also taken into account by issuing an uncertainty for the frame transformation. The developed hardware requires calibration. Methods for calibration are not included in this paper but one must aware of this issue. The calibration method of our case is described in .
The presented low-cost 3D Lidar has been used for mapping and path planning for autonomous mobile machines. The mapping method uses the servo angle and localization data to transform the laser range data into 3D point cloud in a global frame. Each point is also assigned with covariance data. The covariance for each point is calculated from the uncertainty data that is related to each coordinated transformation. A chain of transformations needs to be done in order to transform the range measurement of the laser scanner into a 3D point (x, y, z position) in the global frame. Each of these transformations has some uncertainty and this uncertainty propagates along the chain of transformations (visible in Figure 3).
Point cloud is then processed to the height map and further to the obstacle map. The mapping method and experiments are presented in . An example point cloud and a related obstacle map are shown in Figure 8. The point cloud was created with the presented low-cost 3D Lidar system installed on a GIM-machine that is a modified multipurpose loader presented in Figure 8. The path driven with the GIM machine is presented as a red line in the point cloud data. One can see that the red area in the point cloud data is recognized as an occupied area in the obstacle map. The produced obstacle map can then be used for path planning as presented in .
Antti Kolu, Kimmo Rajapolvi, Mika Hyvönen, Petteri Multanen, Kalevi Huhtala, department of intelligent hydraulics and automation, Tampere University of Technology (TUT). This article originally appeared in the CAN in Automation (CAN) Newsletter magazine. CiA is a CFE Media content partner. Edited by Chris Vavra, production editor, Control Engineering, CFE Media, email@example.com.
 B. Broten and J.Collier. Continuous Motion, Outdoor 2 ½ Grid Map Generation using an Inexpensive Nodding 2-D Laser Rangefinder. Proceedings of the 2006 IEEE International Conference on Robotics and Automation. Orlando, Florida – May 2006.
 R. Ghabcheloo., M. Hyvönen, J. Uusisalo, O. Karhu, J. Järä and K. Huhtala, 2009. Autonomous motion control of a wheel loader. Proceedings of the ASME 2009 Dynamic Systems and Control Conference (DSCC2009), Hollywood, California, USA 8 p., October 12-14, 2009.
 O. Stemme and P. Wolf. Principles and Properties of Highly Dynamic DC Miniature Motors.
 H. Rasmussen. Automatic Tuning of PID-regulators.
 Sick Sensor Intelligence. Sick LMS111-10100 Product pages.
 Maxon Motor Ag. EC-max 30 product catalog.
 A. Kolu, M. Lauri, M. Hyvönen, R. Ghabcheloo and K. Huhtala. A mapping method tolerant to calibration and localization errors based on tilting 2D laser scanner. European control conference (ECC15), Linz, Austria, 2015.
 Ji-Wung Choi and K. Huhtala. Constrained Path optimization with Bezier Curve Primitives. Intelligent Robots and Systems (IROS 2014). Chicago, USA, p. 246 – 251, 2014.