16 CAN Newsletter 4/2016

Autonomous mobile work machines require accurate maps of their surroundings in order to be able to per-

form 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 envi-ronment. As the machines are constantly moving, this infor-mation needs to be constantly updated.

Some commercial solutions of 3D Lidars can be found on the market, for example from manufacturers such as Sick [6] and Velodyne [10], but in many cases these are quite expensive, especially for outdoor applications. Therefore, there is a need for a robust low cost 3D Lidar, especially for use in research. We present a solution for a low cost 3D Lidar based on a 2D laser scanner, an electric motor drive rotating the 2D scanner, and data fusion with the navi-gation system of the mobile machine. The 2D laser scan-ner 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 sens-ing and mapping the environment of an autonomous mobile machine. In this article, the mechanics and control system structure of the servo are presented in detail to make build-ing similar types of systems easier for the interested reader. The developed hardware described in this article was made for the laser mapping of the autonomous mobile machine used in the GIM project [2]. 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 con-trol system, electronics, and hydraulics have been changed to enable researching autonomous operations. The control system architecture of this autonomous machine is illus-trated in Figure 1. The hardware devices are located phys-ically in two different locations. Visualization and operator computers are off-board computers and those are con-

3D Lidar for mobile machinery

Autonomous mobile work machines need the capability of sensing and mapping the surrounding area. Finnish researchers have developed 3D Lidar based on a 2D laser scanner and electric motor drive that rotates the scanner.

nected 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 embed-ded vehicle computers, which communicate with the mid-dle level control through four CAN networks. The imple-mentation of CAN followed the CANopen standardization, which enables fluent CAN network management and con-sistency handling.

The middle level control, including navigation, path plan-ning, 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 – was 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 for the data transfer from the 2D laser scanner to the middle level

Figure 1: Control system architecture of autonomous mobile machine [3] (Photo: TUT)

Sens

ors

control to ensure a real-time data transfer of the laser scanner data.

Electric motor drives and 2D 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 in-termediate circuit. In accurate position and speed electric motor drives, BLDC motors are more common. [4] Thus, a BLDC motor was also used in this case. A ceramic plane-tary gear was used for the reduction of the output rotation speed and to increase the output torque.

Feedback sensors

The accurate and not (or in practice minimum and constantly) delayed mea-surement of the rotation angle is essen-tial to later enable data fusion with la-ser scanner range data for creating a 3D point cloud data. This is more im-portant 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 3D 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 FRPPXWDWLRQ�GRHVQ·W�XVH�VHQVRUV�DW�DOO��VL[�VWHS�FRPPX-tation uses Hall sensors, and sine commutation uses for example incremental encoders for commutation. Sine com-mutation is the most recommended commutation type if the application requires a constant torque generation over the whole rotation speed range [7]. Thus, an external incre-mental encoder was selected for measuring the speed of the electric motor for speed control purposes. The mea-surement of the incremental encoder is a relative measure-ment, referenced to a certain reference rotation angle of

Figure 2: Control system with two control loops (Photo: TUT)

����������������������������������

��(FRQRPLFDO�VROXWLRQV�IRU�VHULHV�DSSOLFDWLRQV��2SWLPL]HG�IRU�LQGXVWULDO�DSSOLFDWLRQV��6ROXWLRQV�IRU�VWDWLRQDU\�DQG�PRELOH�XVH��6RIWZDUH�VXSSRUW�IRU�EXV�DQDO\VLV�PHDVXUHPHQW�DQG�FRQWURO

6RQQHQKDQJ��'�������,OPP�QVWHU7HO��������������������)D[������������������ZZZ�HPV�ZXHQVFKH�FRP

(WKHU&$1�&,�$50�&7UDQV�2/ &3&�86%�HPEHGGHG(WKHU&$1�&,�$50� &3&�86%�HPEHGGHG

18 CAN Newsletter 4/2016

URWRU��,W�FDQ·W�EH�GLUHFWO\�XVHG�DV�D�URWDWLRQ�DQJOH�PHDVXUH-ment 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 measur-ing the rotation angle despite the fact that absolute encod-ers are typically more expensive than incremental encod-ers 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 con-troller is based on two control loops as illustrated in Fig-ure 2. Two control loops were used because the system includes a planetary gear that brings backlashes to the sys-tem under control. Furthermore, backlashes cause delays in control systems and 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 sen-sor in the auxiliary control loop is an incremental encoder that is located at the backend of the electric motor. The pri-mary 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 sam-pling time; the speed controller is implemented as a PI con-troller 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 accelera-tion 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 soft-ware that can be used for controller tuning.

Property Value

Angle resolution 0.25° / 0.5°

Output frequency 25 Hz / 50 Hz

Operating range max 20 m

Field of view (FOV) max 270°

Systematic error ± 30 mm

Statistical error Typical 12 mm

Table 1. Properties of a Sick LMS111-10100 laser scanner

Figure 3: 2D laser scanner (Sick LMS111-10100) installed on an autonomous mobile machine (Photo: TUT)

Property Value

Nominal voltage 24 V

No load speed 9250 rpm

No load current 123 mA

Nominal speed 7230 rpm

Nominal torque 33,6 mNm

Nominal current 1,48 A

Speed / torque gradient 59,1 rpm / mNm

Speed constant 393 rpm / V

Torque constant 24,3 mNm / A

Table 2: Parameters of the electric motor [7]

Component Manufacturer Component type Details

Electric motor controller Maxon Motor Ag EPOS2 50/5 Support for dual loop control

Incremental encoder Maxon Motor Ag Encoder MR Type ML 1000 CPT with index chan-nel

Absolute encoder Scancon Industrial Encoders

SAG-S101G-0016- C06S-PAL

Resolution 16 bit, SSI interface to motor controller

Electric motor Maxon Motor Ag EC-max 30 Build-in Hall sensors

Planetary gear Maxon Motor Ag Ceramic Planetary Gearhead GP 32 C Gear ratio 1181:1

Shunt regulator Maxon Motor Ag DSR 50/5 -

Choke Module Maxon Motor Ag Choke module -

Table 3: Components of the electric motor drive

Sens

ors

www.kvaser.com

Kvaser CAN Solutions

93 Products Limitless Possibilities

WHY CHOOSE A KVASER CAN INTERFACE?

The Widest Range of CAN and CAN FD Interfaces and Dataloggers, Plus:

d &QEEØ3TOONQS�Ø3NàVAQEØAMDØ3NàVAQEØ5ODASERØFNQØ,IFE

d !Ø5MIUEQRAKØ!0)

d !MØ/OEMØ%CNRXRSELØVISH %ARXØ4NNKCHAIMØ)MSEGQASINM

d !Ø.ESVNQJØNFØNUEQØ���Ø3NàVAQEØ0AQSMEQRØAMDØ1TAKIÜEDØ3AKERØ2EOQEREMSASIUER

KVR-6044_Print_Ad_Final_V3.indd 1 8/23/16 2:59 PM

2D laser scanners

The purpose of the electric motor drive is to continuous-ly rotate a 2D 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 Fig-ure 3. The measurements in Figure 6 illustrate how the posi-tion (left) and angular speed (right) behave over one move-ment 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 2D laser scanner is based on the time-of-flight (TOF). The 2D laser scanner gives range and intensity values as measurement values from the measured plane. Measured range values are relative to the rotating mir-ror of the 2D laser scanner and for every measured point one can also get the intensity value [6].

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

Figure 4: Control unit of electric motor (left) and the schematics and a photo of the 2D laser scanner rotation unit without casing (right) (Photo: TUT)

Figure 5: Flow chart of developed software (Photo: TUT)

20 CAN Newsletter 4/2016

drive and planetary gear. The electric motor has the parame-ters 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 reg-ulators 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 differ-ent 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 2D laser scanner is supported from both sides with slide bearings. These slide bearings are marked yel-low in Figure 4. Slide bearings support the mechanical structure so that an axial movement of the 2D laser scan-ner is prohibited. Another purpose of the slide bearings is WR� WROHUDWH�D�UDGLDO� ORDG��7KH�VOLGH�EHDULQJV·�PDWHULDO� LV�D�brass alloy and the axle material against this brass alloy is Hydax 15 steel.

The mechanical structure and manufacturing tech-nique might cause axial misalignment, so components that fit the axial misalignment are needed. Mechanical coupling on both side of the 2D laser scanner makes this axial mis-alignment fitting, and mechanical coupling forward rota-tional 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 2D laser scanner are controlled by a smaller control unit seen in Figure 4, which was integrated in an existing control sys-tem. The execution of the developed software follows the flow chart as illustrated in Figure 5.

In the initialization phase, the electric motor control-ler and 2D 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 2D laser scanner, the FOV (field of view) 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 addi-tion, 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 2D laser

Figure 6: Measured position (left) and angular speed (right) of the electric motor (Photo: TUT)

Figure 7: 2D FOV of the laser scanner used in the mapping (red) and the region when laser scanner does the UDP packet sending (blue) (Photo: TUT)

Sens

ors

INTERACT IV ITY

CONNECT IV I TY

group

HeadquartersVia Tito Speri, 1025024 LENO (Brescia) ITALY Ph +39 030 904511Fax +39 030 9045330

info@cobogroup.netwww.cobogroup.net

CONNECT AND MANAGE YOUR VEHICLES WHERE AND WHEN YOU WANT

TERA 12 HE12.1” TFT Advanced Display

CANVIEW 44.3” TFT Display

CANLive

WiPass CAN

scanner to a certain angle according to the given param-eters and “Sweep between angles” rotates the 2D laser scanner between certain angles with the given parame-ters. Both operating modes use the electric motor control-OHU·V� LQWHUQDO� RSHUDWLQJ�PRGH� ´3URILOH�3RVLWLRQ�0RGHµ� >�@��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 devel-oped hardware uses both networks to transfer measure-ment data. Thus, the measurement data must be synchro-nized 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 2D 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 measure-ment data can be time synchronized. This requires that the clocks of both devices are synchronized frequently enough to prevent time drifting.

6RPH�GHYLFHV�GRQ·W�SURYLGH�WLPH�VWDPSV�IRU�WKHLU�PHD-surement 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 mea-surement 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 2D laser scanner sends the measure-ment data with a constant time interval, which is 25 Hz or 50 Hz. The measurement data is transferred from the 2D 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 2D laser scanner is a real time output [6]. Thus, it is assumed to work in such a way that the 2D laser scanner measures the range data and sends the measured values to the middle level con-trol through UDP with a low network latency right after mea-surement. 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 2D 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 measure-ment, 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

22 CAN Newsletter 4/2016

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 2D laser scanner also affects the synchronization error. The internal rotating mir-ror of the 2D laser scanner rotates with 25 Hz or 50 Hz so one cycle takes 40 ms or 20 ms. The UDP packet that con-tains 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 2D laser scanner does not measure. This UHJLRQ�LV�PDUNHG�EOXH�LQ�)LJXUH����2QH�FDQ·W�NQRZ�WKLV�WLPH�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 measure-ment frequency is 50 Hz and FOV is 140 degrees. Figure 7 illustrates the region in red that the 2D 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 max-imum error of 15 ms time synchronization can exist between the last measured point and the rotation angle of the 2D laser scanner.

The time difference between the measured range val-ues 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 rotat-ing mirror and the rotation angle of the servo of the 2D 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 synchro-nization of the frame transformation. The coordination is needed between the rotating mirror of the 2D laser scanner and the axle of the electric motor drive. The related frame transformations are calculated in [8], where the motion syn-chronization is also taken into account by issuing an uncer-tainty 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 [8].

Figure 8: Point cloud created with low-cost Lidar installed on a GIM-machine and obstacle map created from the point cloud in question; the wall is presented in the point cloud with red and the wall is also recognized as an obstacle, shown black in obstacle map (Photo: TUT)

Authors

Antti Kolu, Mika Hyvönen, Petteri Multanen, Kalevi Huhtala, Kimmo RajapolviDepartment of Intelligent Hydraulics and Automation Tampere University of Technologyantti.kolu@tut.fiwww.tut.fiReferences

Mapping

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 glob-al frame. Each point is also assigned with covariance data. The covariance for each point is calculated from the uncer-tainty data that is related to each coordinated transformation. A chain of transformations needs to be done in order to trans-form the range measurement of the laser scanner into a 3D point (x,y,z position) in the global frame. Each of these trans-formations has some uncertainty and this uncertainty propa-gates along the chain of transformations (visible in Figure 3).

The point cloud is then processed to the height map and further to the obstacle map. The mapping method and exper-iments are presented in [8]. 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 rec-ognized as an occupied area in the obstacle map. The pro-duced obstacle map can then be used for path planning as presented in [9].

Acknowledgement: This work was supported by the Academy of Finland through the General Intelligent Machines (GIM) project. W

Sens

ors

Projekt2 02.11.15 09:01 Seite 1