Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters

Krzysztof Naus; Łukasz Marchel

doi:10.3390/app9173530

Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters

Naus, Krzysztof;Marchel, Łukasz 2019-08-28 00:00:00 applied sciences Article Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters Krzysztof Naus * and Łukasz Marchel * Institute of Navigation and Maritime Hydrography, Polish Naval Academy, 81-103 Gdynia, Poland * Correspondence: k.naus@amw.gdynia.pl (K.N.); l.marchel@amw.gdynia.pl (Ł.M.) Received: 1 August 2019; Accepted: 23 August 2019; Published: 28 August 2019 Abstract: The purpose of this article is to present a study aimed at developing a method for the precise determination of unmanned surface vehicle (USV) movement parameters (heading (HDG), speed over ground (SOG) and rate of turn (ROT)) through appropriate processing. The technique employs a modiﬁed weighted ICP (Iterative Closest Point) algorithm and a 2D points layer arranged in the horizon plane obtained from measurements. This is performed with the help of Light Detection and Ranging (LIDAR). A new method of weighting is presented. It is based on a mean error in a given direction and the results of modiﬁed weighted ICP tests carried out on the basis of ﬁeld measurement data. The ﬁrst part of the paper characterizes LIDAR measuring errors and indicates the possibilities for their use in matching point clouds. The second part of the article deals with a method for determining the SOG and course over ground (COG), based on a modiﬁed weighted ICP algorithm. The main part of the paper reviews a test method aimed at evaluating the accuracy of determining the SOG and COG by the scan-matching method using a modiﬁed weighted ICP algorithm. The ﬁnal part presents an analysis comparing the obtained SOG and COG results with reference results of GNSS RTK measurements and the resulting generalised conclusions. Keywords: ICP; mean error ellipse; precise maneuvering; movement parameters 1. Introduction Heading (HDG), speed over ground (SOG) and rate of turn (ROT) are basic movement parameters which characterize the way a ship’s hull moves in relation to the Earth’s surface. They are crucial in the decision-making process, particularly when navigating in congested areas, including harbors and rivers. Currently, these parameters are most frequently determined on a ship using a gyrocompass and a log, i.e., navigational instruments whose measuring accuracy, according to the International Maritime Organization (IMO) requirements, is not required to be very high: “Errors in the indicated speed, when the ship is operating free from shallow water eect and from the eects of wind, current and tide, should not exceed 2% of the speed of the ship, or 0.2 knots, whichever is greater” [1], “The follow-up error for dierent rates of turn should be: less than 0.5 at rates up to 10 /s; and less than 1.5 between a rate of 10 /s and 20 /s” [2], although it determines the precision and the ability to control the ship’s movement which, consequently, may aect the navigational safety level. It is therefore reasonable to carry out scientiﬁc research aimed at searching for new measuring and data processing methods in order to minimize measuring errors in a ship’s movement parameters. For many years, maritime navigation has placed increasing emphasis on the development of methods to determine position and navigational parameters that could oer an alternative to the GNSS system [3]. In the age of the development of unmanned surface vehicles (USVs) whose main tasks increasingly involve independent (autonomous) maneuvering in restricted areas, it is necessary to develop new methods to obtain precise (accurate) navigational movement parameters (in particular the HDG, SOG, course over ground (COG), and ROT) [4,5]. Basing the USV decision-making process Appl. Sci. 2019, 9, 3530; doi:10.3390/app9173530 www.mdpi.com/journal/applsci Appl. Sci. 2019, 9, 3530 2 of 17 exclusively on the parameters of movement and positions obtained from satellite systems is unjustiﬁed, particularly in port basins in which infrastructure facilities and other ships may suppress and reﬂect the system signals [6–8]. Another reason why one should not rely exclusively on such solutions is the new techniques of satellite signal jamming and spooﬁng [9]. A good way to avoid the satellite system inconveniences may be the development of locally operating systems whose actuator is located on a carrier ship, making their operation more dicult to interfere with. A system with a local range of operation needs no special, external infrastructure to generate correct data. Recently, the clear development of devices such as Light Detection and Ranging (LIDAR) [10] has been noted, with increased capacities in terms of the range, accuracy of generated data and the purchase price. Certain devices have a range above 500 m and high accuracy in angle and distance measurements, which enables more widespread use of such devices in sea-going ship navigation. It should also be noted that the measuring capacity of such sensors is not limited by the radar ’s dead zone at close ranges. On the other hand, such a phenomenon is typical of pulse radars which are commonly used on almost every vessel [11]. This article presents a method for the precise determination of the SOG and COG parameters through processing, using a modiﬁed iterative closest point (ICP) algorithm [12–15] of a horizontal 2D layer of cloud points obtained from LIDAR measurements. The main process is based on the simultaneous localization and mapping (SLAM) method. In simple terms, this is a method allowing a map of an unknown environment to be built, which, after processing, is used for navigating in a particular area. This technique was isolated as a separated area of interest in 1996. The SLAM evolution was described in [16]. SLAM is rarely used in determining the position of surface vessels. Only a few applications of the method in the context of its use on an open sea surface are known [17]. On the other hand, SLAM has become very popular in determining the position of autonomous underwater vehicles (AUV). A review of the methods applied in determining the positions of AUVs can be found in [18]. Since SLAM is already a relatively well-studied method, many approaches to solving a particular position-determining problem have emerged. One of the most common solutions to the problem is the so-called scan-matching. This set of techniques involves matching the subsequent scans of the surrounding environment which are obtained while exploring a particular area [19]. There are numerous algorithms variants used to match scans obtained by mapping devices. These include the ICP algorithm [20,21], ICL [22] and algorithms based on matching histograms of the distance function [23]. However, given the dierent methods of matching point clouds, the ICP algorithm proposed for the ﬁrst time in [24] outperforms other algorithms in its simplicity and the possibility for its continuous development. Considering the use of the SLAM technique to determine the parameters of the vessel’s movement, it is worth looking at the 2011 article by Callmer et al. [25]. In this case, the authors use the object approach (landmark) based on the SIFT algorithm and the extended Kalman ﬁlter (EKF), and images from the navigation radar. The results of the work are promising; however, in this case there is a signiﬁcant drift of coordinates of the position and parameters of the vessels movement. Another publication describing the implementation of the SLAM task at sea [17]. The approach presented here assumes that the map of the environment will be explored on a simultaneous basis by more than one vehicle/ship. This assumption allows obtaining a much more accurate model of the map of the environment (global map) by combining many sub-maps obtained by vehicles. This mapping is used to position each of them. The approach presented in the article is a scan matching without odometry information. A similar solution, but based on another scan matching algorithm, was presented in [26]. It involves adjusting concurrent scans one after another without using dead reckoning data in the alignment algorithm. This article uses the ICP algorithm with the error metric in the point-to-point version [27]. Other versions of the metrics are: point-to-line [28], point-to-plane [29], and plane-to-plane [30]. The point-to-point metric was chosen because of the intuitive way of attaching importance to individual correspondences Appl. Sci. 2019, 9, 3530 3 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 3 of 17 (based solely on the coordinate error of the position of a pair of points), without the need to consider need to consider the uncertainty of the position of the line or plane (for cases of three-dimensional the uncertainty of the position of the line or plane (for cases of three-dimensional point clouds). point clouds). In this article, the emphasis is put on the methods of weighting on the basis of estimated errors In this article, the emphasis is put on the methods of weighting on the basis of estimated errors of the position coordinates of a pair of corresponding points. The problem of weighting of pairs in of the position coordinates of a pair of corresponding points. The problem of weighting of pairs in a a navigational approach is presented in [31], and for other applications in [32,33]. This publication navigational approach is presented in [31], and for other applications in [32, 33]. This publication assumes that pairs with a higher distance value between points will be given lower weights compared assumes that pairs with a higher distance value between points will be given lower weights compared to those at a smaller distance from one another. Weighting based on color was also accepted. Another to those at a smaller distance from one another. Weighting based on color was also accepted. Another approach presented in [21] is the weighting depending on the uncertainty of the position of points in approach presented in [21] is the weighting depending on the uncertainty of the position of points in the scan and the uncertainty of the position of the straight line (in the point-to-line variant). It was the scan and the uncertainty of the position of the straight line (in the point-to-line variant). It was assumed that more weight will be put on pairs that have less inﬂuence on the error metric. A similar assumed that more weight will be put on pairs that have less influence on the error metric. A similar approach is presented in [34]. The authors carry out tests on three variants of the error measure: point, approach is presented in [34]. The authors carry out tests on three variants of the error measure: point, line and plane. The approach proposed in this article is similar to this; however, it assumes a dierent, line and plane. The approach proposed in this article is similar to this; however, it assumes a different, more universal distribution of coordinates error. They can be easily implemented not only in the case more universal distribution of coordinates error. They can be easily implemented not only in the case of a lidar sensor, but also in the case of navigation radar. of a lidar sensor, but also in the case of navigation radar. 2. Evaluation of the Accuracy of Determining the Coordinates of a Position Using LIDAR 2. Evaluation of the Accuracy of Determining the Coordinates of a Position Using LIDAR The determination of horizontal coordinates using LIDAR is carried out based on the angle The determination of horizontal coordinates using LIDAR is carried out based on the angle 𝛼 and the distance r measured between the sensor and the target, i.e., the light signal reﬂection point and the distance 𝑟 measured between the sensor and the target, i.e., the light signal reflection point (Figure 1). (Figure 1). Fig Figure ure 1. 1. The The prin principle ciple of m of eameasuring suring 2D h2D oriz horizontal ontal coordi coor natdinates es (x, y)( x usi , y n)gusing Light Light DetectDetection ion and Ra and nging Ranging (LIDAR). (LIDAR) The mean error M and the parameters of mean error ellipse (i.e., the lengths of semi-axes 𝑎 and The mean error M and the parameters of mean error ellipse (i.e., the lengths of semi-axes a and b) 𝑏 ) of the determination of 2D horizontal coordinates (x, y) using LIDAR can be calculated by ( ) of the determination of 2D horizontal coordinates x, y using LIDAR can be calculated by applying applying the law of mean error propagation [35, 36]. When the functions of a single result of the the law of mean error propagation [35,36]. When the functions of a single result of the following following measurement are known: measurement are known: = arc tg , (1) 𝛼 = arc tg , (1) 2 2 (2) 𝑟 = √𝑥 + 𝑦 , Appl. Sci. 2019, 9, 3530 4 of 17 2 2 r = x + y , (2) equations of the mean errors of positional lines can be written as: ! ! 2 2 @ @ = / + , (3) l() @x @y ! ! 2 2 @r @r = / + , (4) l(r) @x @y which enable the determination of mean error of position: 2 2 2 M = + = ( r) + , (5) l() l(r) sin a = r, (6) b = , (7) where: —the angle of intersection of positional lines, which for LIDAR measurements is always equal to 90 , —mean error of the angle measurement , —mean error of the distance measurement r, r—distance between the sensor and the target, a—the length of the long semi-axis of mean error ellipse (see Figure 2), b—the length of the short semi-axis of mean error ellipse (see Figure 2). It follows from Dependence (5) that the value of mean error M and the length of the long semi-axis a of the mean error ellipse are signiﬁcantly aected by the distance r between the sensor and the target. The calculations assumed that the measurements were performed using the Scanse Sweep LIDAR [37] for which the manufacturer provides = 0.004258r (rounded value). Thereby one takes into account the fact that the error value of the angle measurement does not only depend on the beam’s divergence (footprint of the beam increases as a function of divergence) and the angle of its incidence on the surface of the object, but also on the accuracy of measuring the angle change by the optical head encoder that can be aected by environmental conditions such as temperature and humidity, vibrations and imperfections in the performance of mechanical elements that cooperate with each other. This can be characteristic for entry level LIDAR devices. The spatial angle resolution can be derived from sampling frequency that is set up to 1000 Hz (when optical sensor is spinning with frequency 1 Hz the Lidar is able to sample up to 1000 point for a single spin, 5 Hz—200 points, 10 Hz—100 points). The range spatial resolution is around 0.1 cm. Based on the known lengths of semi-axes a and b of the mean error ellipse, and based on the known direction of measurement, one can simply generate a covariance matrix cov of the (x, y) vector coordinates: 2 3 2 2 2 2 2 6 a cos + b sin a b sin cos 7 a a a a 6 7 6 7 cov(x, y) = 6 7, (8) 4 5 2 2 2 2 2 2 a b sin cos a sin + b cos a a a a where —is the angle between the semi-axis a of the mean error ellipse and the axis OY (for LIDAR measurements, equal to + 90 ). Then, using this, to determine the mean square error of the determination of the position coordinates, but in the speciﬁed direction : Appl. Sci. 2019, 9, 3530 5 of 17 2 T 2 2 2 2 = S cov(x, y)S = sin sin r sin + cos a r a 2 2 2 cos cos sin r + a a r (9) 2 2 2 2 + sin sin r cos + sin a r a 2 2 2 cos sin sin r , a a r h i where S = cos sin . Appl. Sci. 2019, 9, x FOR PEER REVIEW 5 of 17 Figure 2. Mean error of the determination of position coordinates in the specified direction. Figure 2. Mean error of the determination of position coordinates in the speciﬁed direction. (calculated (calculated using the covariance matrix). using the covariance matrix). 2.1. The Determination of USV Movement Parameters Using the Scan-Matching Method 2.1. The Determination of USV Movement Parameters Using the Scan-Matching Method The taxonomy of the ICP algorithm is carried out in the following successive stages of data The taxonomy of the ICP algorithm is carried out in the following successive stages of data processing [21]: processing [21]: Selection (selection of points which are suitable for the alignment); • Selection (selection of points which are suitable for the alignment); Matching (matching points from model cloud to reference points cloud); • Matching (matching points from model cloud to reference points cloud); • Weighting (giving weight to corresponding points); Weighting (giving weight to corresponding points); • Rejecting (deleting some points based on the robust criterion function); Rejecting (deleting some points based on the robust criterion function); • Assigning an error metric (choosing point-to-point, point-to-line, point-to-plane or plane-to- Assigning an error metric (choosing point-to-point, point-to-line, point-to-plane or plane-to-plane plane error metric); error metric); • Minimizing the metric between selected points. Minimizing the metric between selected points. Selection. Let us assume that a USV determines the change in its position through the translation Selection. Let us assume that a USV determines the change in its position through the translation vector 𝐓 (𝑘 + 1) and the rotation matrix 𝐑 (𝑘 + 1) determined based on two maps of the ( ) ( ) vector T k + 1 and the rotation matrix R k + 1 determined based on two maps of the surrounding surrounding environment 𝐌 (𝑘 ) and 𝐌 (𝑘 + 1), constructed at the moment (𝑘 ) and the moment environment M(k) and M(k + 1), constructed at the moment (k) and the moment (k + 1) following it ( ) ( ) 𝑘 + 1 following it (e.g., 2D horizontal layer of cloud points for LIDAR measurements). Let ℤ 𝑘 ( ) ( ) (e.g., 2D horizontal layer of cloud points for LIDAR measurements). Let Z k and Z k + 1 represent and ℤ(𝑘 + 1) represent two sets {(𝑟 (𝑘 ), (𝑘 )), … , (𝑟 (𝑘 ), (𝑘 ))} and {(𝑟 (𝑘 + 1), (𝑘 + 1 1 𝑛 𝑛 1 1 two sets (r (k), (k)), ::: , (r (k), (k)) and (r (k + 1), (k + 1)), ::: , (r (k + 1), (k + 1)) of 1 1 n n 1 1 n n ) ( ) ( ) 1 ), … , (𝑟 𝑘 + 1 , 𝑘 + 1 )} of measurements of the distance 𝑟 and direction  , carried out in 𝑛 𝑛 measurements of the distance r and direction , carried out in relation to n ﬁeld obstacles (see Figure 1). n o n o relation to 𝑛 field obstacles (see Figure 1). On their basis, set of points 𝐌 (𝑘 ) = {𝐩 (𝑘 ), … , 𝐩 (𝑘 )} and 1 n On their basis, set of points M(k) = p (k), ::: , p (k) and M(k + 1) = p (k + 1), ::: , p (k + 1) 1 n 1 n { } 𝐌 (𝑘 + 1) = 𝐩 (𝑘 + 1), … , 𝐩 (𝑘 + 1) will be determined, where the coordinates of each point in the 1 n will be determined, where the coordinates of each point in the formula are calculated via the formula are calculated via the following dependence: following dependence: " # 𝐩 = [ ], p = , i 𝑦 (10) x = r cos( ), i i i (10) 𝑥 = 𝑟 ∙ cos ( ), 𝑖 𝑖 𝑖 y = r sin( ), i i i 𝑦 = 𝑟 ∙ sin( ), 𝑖 𝑖 𝑖 where: i = 1,::: , n where: ( ) ( ) They will be used to determine the translation vector T k + 1 and the rotation matrix R k + 1 𝑖 = 1, … , 𝑛 using the singular value decomposition (SVD) method [38], which will be carried out in combination They will be used to determine the translation vector 𝐓 (𝑘 + 1) and the rotation matrix 𝐑 (𝑘 + 1) using the singular value decomposition (SVD) method [38], which will be carried out in combination with a change in the position (in subsequent iteration steps) of the set of points from 𝐌 (𝑘 ) to 𝐌 (𝑘 + 1) using the modified weighted ICP method. In reality, there is very little probability that both sets of points 𝐌 (𝑘 ) and 𝐌 (𝑘 + 1) will contain the same number of points. Equalising the number of points is required for the family of ICP algorithms with point-to-point error metric and therefore, in the calculations they were carried out by reducing the number of points. From a more numerous set, the points were randomly removed so that their number would be equal to the number of the points from a smaller set. Appl. Sci. 2019, 9, 3530 6 of 17 with a change in the position (in subsequent iteration steps) of the set of points from M(k) to M(k + 1) using the modiﬁed weighted ICP method. In reality, there is very little probability that both sets of points M(k) and M(k + 1) will contain the same number of points. Equalising the number of points is required for the family of ICP algorithms with point-to-point error metric and therefore, in the calculations they were carried out by reducing the number of points. From a more numerous set, the points were randomly removed so that their number would be equal to the number of the points from Appl. Sci. 2019, 9, x FOR PEER REVIEW 6 of 17 a smaller set. Matching. Literature shows that there are many ways to match points in pairs. One of them Matching. Literature shows that there are many ways to match points in pairs. One of them is is the k-d tree method presented in [39,40]. Also considered are methods based on the heuristic the k-d tree method presented in [39,40]. Also considered are methods based on the heuristic search search of pairs to reduce the complexity of calculations [41]. In this article, correspondence between of pairs to reduce the complexity of calculations [41]. In this article, correspondence between points points is created based on the method presented in [42]. It is based on the Delaunay triangulation. is created based on the method presented in [42]. It is based on the Delaunay triangulation. The The method generates a two-dimensional grid of triangles based on the reference set M(k). Next, using ( ) method generates a two-dimensional grid of triangles based on the reference set 𝐌 𝑘 . Next, using ( ) the nearest neighbor method, the nearest apex of triangles to the points from the set M k + 1 are found the nearest neighbor method, the nearest apex of triangles to the points from the set 𝐌 (𝑘 + 1) are (points from M(k)) in such a way that for each point p (k) the nearest point p (k + 1) is assigned i id(i) ( ) ( ) ( found (points from 𝐌 𝑘 ) in such a way that for each point 𝐩 𝑘 the nearest point 𝐩 𝑘 + 𝒊 𝒊𝒅 (𝒊 ) (see Figure 3). 1 is assigned (see Figure 3). Figure 3. Left side—2-point clouds before Dalaunay triangulation; right side—Dalaunay triangulation Figure 3. Left side—2-point clouds before Dalaunay triangulation; right side—Dalaunay triangulation and nearest neighbor match (dashed lines). and nearest neighbor match (dashed lines). Weighting of the pairs. The modiﬁcation of the ICP method will involve the application of the Weighting of the pairs. The modification of the ICP method will involve the application of the weighting factor w (k, k + 1) for calculations for each pair of points (nearest neighbors): 1,:::,n weighting factor 𝑤 (𝑘 , 𝑘 + 1) for calculations for each pair of points (nearest neighbors): 1,…,𝑛 1 1 𝑤 (𝑘 , 𝑘 + 1) = , w (k, k + 1) = p , ( (11) 11) √𝐺 (𝑘 ,𝑘 +1) G (k, k + 1) ( ) ( ) where 𝐺 𝑘 , 𝑘 + 1 is the sum of mean errors of the determination of the coordinates of points 𝐩 𝑘 𝑖 𝑖 where G (k, k + 1) is the sum of mean errors of the determination of the coordinates of points p (k) and 𝐩 (𝑘 + 1) in the specified direction 𝛽 (see Figure 2): (𝑖 ) and p (k + 1) in the speciﬁed direction (see Figure 2): id(i) 2 2 𝐺 (𝑘 , 𝑘 + 1) = 𝜎 (𝑘 ) + 𝜎 (𝑘 + 1), 𝑖 𝛽 𝛽 (12) 𝑖 (𝑖 ) 2 2 G (k, k + 1) = (k) + (k + 1), (12) id(i) where 𝜎 (𝑘 ) is the mean error of the determination of the coordinates of the point 𝐩 (𝑘 ) in the 𝛽 𝑖 direction of 𝛽 (from 𝐩 (𝑘 ) to 𝐩 (𝑘 + 1)), 𝜎 (𝑘 + 1) is the mean error of the determination of 𝑖 (𝑖 ) 𝛽 where (k) is the mean error of the determination of the coordinates of the point p (k) in the direction (𝑖 ) of th e (fr co om ord pin(a ktes ) to o pf th(e k + poi 1) n)t, 𝐩 (𝑘 + (k1+ ) 1i) n isth the e d mean irectio err nor of of 𝛽 the (fdetermination rom 𝐩 (𝑘 + of 1) the tocoor 𝐩 ( dinates 𝑘 )) , or 𝑖 (𝑖 ) 𝑖 i id(i) id(i) of 𝐺 the (𝑘 , poi 𝑘 +nt 1) p i(s k + the 1)m in ea the n er dir ro ection r of th ofe d (fr ete om rmi pnati(o kn+ o1 f) th toe pcoo (k)rd ), or ina G tes (k,ok f + th1 e ) po is the intsmean 𝐩 (𝑘 ) err aor nd 𝑖 i 𝑖 i id(i) i of the ( determination ) of the coordinates of the points p (k) and p (k + 1) (see Figure 1): 𝐩 𝑘 + 1 (see Figure 1): (𝑖 ) i id(i) 2 2 2 2 ( ) ( ) ( ) G 𝐺 (k𝑘, k , 𝑘 ++ 1) 1== M 𝑀 (k) 𝑘 ++ M 𝑀 (k 𝑘++ 1) 1 , , ( (13) 13) i 𝑖 𝑖 (𝑖 ) id(i) where 𝑀 (𝑘 ) is the mean error of the determination of the coordinates of the point 𝐩 (𝑘 ) , 𝑖 𝑖 where M (k) is the mean error of the determination of the coordinates of the point p (k), M (k + 1) i id(i) ( ) ( ) 𝑀 𝑘 + 1 is the mean error of the determination of the coordinates of the point 𝐩 𝑘 + 1 . (𝑖 ) (𝑖 ) ( ) is the mean error of the determination of the coordinates of the point p k + 1 . id(i) Weighting step gives specific calculated value for each pair. Then in further processing heavier pairs involve more the translation and rotation parameters (Figure 4). 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 Appl. Sci. 2019, 9, 3530 7 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 7 of 17 Weighting step gives speciﬁc calculated value for each pair. Then in further processing heavier Appl. Sci. 2019, 9, x FOR PEER REVIEW 7 of 17 pairs involve more the translation and rotation parameters (Figure 4). Figure 4. Weighting due to proposed error factor. Bolted, dashed line equals more weight for specific Figure 4. Weighting due to proposed error factor. Bolted, dashed line equals more weight for pair. Figure 4. Weighting due to proposed error factor. Bolted, dashed line equals more weight for specific speciﬁc pair. pair. Rejection. Due to the relatively high noise level of many scans, particularly in the sectors Rejection. Due to the relatively high noise level of many scans, particularly in the sectors illustrating illustrating long distance measurements, the Huber function [43] which attenuated the outlying Rejection. Due to the relatively high noise level of many scans, particularly in the sectors long distance measurements, the Huber function [43] which attenuated the outlying measurement measurement results (affected by gross errors) was also applied in the calculations. This function was illustrating long distance measurements, the Huber function [43] which attenuated the outlying results (aected by gross errors) was also applied in the calculations. This function was arbitrarily arbitrarily selected from among many functions known and commonly used for this purpose [21, 31, measurement results (affected by gross errors) was also applied in the calculations. This function was selected from among many functions known and commonly used for this purpose [21,31,44–47]. 44 a– rb 47 itra ]. ri The ly sel roec bust ted f fun rom cti a om n ore ng jec m ts a n oy r fs un ign cti ifo ica ns nkn tly ow ren d uce ands co wei mm gh ots nl y oused f the fspe or th ciis fi c pur papo irsse d[ ue 21, t3 o1 , The robust function rejects or signiﬁcantly reduces weights of the speciﬁc pairs due to robustness robustness criterion (see Figure 5). The robust function is based on 𝑘𝐻𝑢 factor presented in 44–47]. The robust function rejects or significantly reduces weights of the specific pairs due to criterion (see Figure 5). The robust function is based on kHu factor presented in Algorithm 1. Rejecting Aro lgo bust rith nm ess 1. cr Rej ite ec rito in ng ( o see r wei Figh gure t m i5 n)i.m The izatio ro nb iust s pe rf fuo n rmed ction bia s sed ba sed on no on rm 𝑘𝐻𝑢 of tw fo a cto pair repr d poi esen nted ts. In i n or weight minimization is performed based on norm of two paired points. In this article the fusion of this article the fusion of the robust function and proposed error weighting is used to derivate the final Algorithm 1. Rejecting or weight minimization is performed based on norm of two paired points. In the robust function and proposed error weighting is used to derivate the ﬁnal weight. wei thigh s at. rti cle the fusion of the robust function and proposed error weighting is used to derivate the final weight. Fig Figure ure 5.5. Re Rejecting jecting pa pairs irs due due to to rob robust ust crcriterion. iterion. Figure 5. Rejecting pairs due to robust criterion. Assigning an error metric. The ﬁnal function minimizing the value of the matching error E, used Assigning an error metric. The final function minimizing the value of the matching error E, used to accurately determine the translation vector T(k + 1) and the rotation matrix R(k + 1), will take the to accurately determine the translation vector 𝐓 (𝑘 + 1) and the rotation matrix 𝐑 (𝑘 + 1), will take Assigning an error metric. The final function minimizing the value of the matching error E, used following form: the following form: to accurately determine the translation vector 𝐓 (𝑘 + 1) and the rotation matrix 𝐑 (𝑘 + 1), will take E(R, T) = w (k, k + 1)kRp (k) + T p (k + 1)k . (14) the following form: 2 i id(i) i=1 (14) E(𝐑 , 𝐓 ) = ∑ 𝑤 (𝑘 , 𝑘 + 1) ∙ ‖𝐑 𝐩 (𝑘 ) + 𝐓 − 𝐩 (𝑘 + 1)‖ . 𝑖 =1 𝑖 𝑖 (𝑖 ) Provided that points p (k) and p (k + 1) are located the closest to one another, i.e., are the E(𝐑 , 𝐓 ) = ∑ 𝑤 (𝑘 , 𝑘 + 1) ∙ ‖𝐑 𝐩 (𝑘 ) + 𝐓 − 𝐩 (𝑘 + 1)‖ . (14) i 𝑖 id(i) 𝑖 (𝑖 ) 𝑖 =1 ( ) ( ) Provided that points 𝐩 𝑘 and 𝐩 𝑘 + 1 are located the closest to one another, i.e., are the 𝑖 (𝑖 ) nearest neighbors. This method is based on the point-to-point measure of error. nearest Pro nei vi gh ded bo rs. tha Thi t po s met ints h 𝐩 o( d 𝑘 i )s b an ad sed 𝐩 on th (𝑘 e + po 1i )n t a-re to -lpoi ocan ted t mea the sure closest of er to ro o r. n e another, i.e., are the 𝑖 (𝑖 ) Minimizing the metric between selected points. The method of minimizing the error between Minimizing the metric between selected points. The method of minimizing the error between nearest neighbors. This method is based on the point-to-point measure of error. the corresponding points was carried out on the basis of the SVD (Singular Value Decomposition). the corresponding points was carried out on the basis of the SVD (Singular Value Decomposition). Minimizing the metric between selected points. The method of minimizing the error between The method works equally for the weighted and classical ICP algorithm. In this article a method The method works equally for the weighted and classical ICP algorithm. In this article a method the corresponding points was carried out on the basis of the SVD (Singular Value Decomposition). identical to [47] was used. It is required to determine two weighted average coordinates from the The method works equally for the weighted and classical ICP algorithm. In this article a method cloud m(𝑘 ) and m(𝑘 + 1), dependent on point clouds (𝑘 ), (𝑘 + 1) and assigned weights in the identical to [47] was used. It is required to determine two weighted average coordinates from the previous stage: cloud m(𝑘 ) and m(𝑘 + 1), dependent on point clouds (𝑘 ), (𝑘 + 1) and assigned weights in the previous stage: 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 Appl. Sci. 2019, 9, 3530 8 of 17 identical to [47] was used. It is required to determine two weighted average coordinates from the cloud m(k) and m(k + 1), dependent on point clouds (k), (k + 1) and assigned weights in the previous stage: Appl. Sci. 2019, 9, x FOR PEER REVIEW 8 of 17 w p (k) i=1 i m(k) = P , (15) 𝑛 i=1 ∑ 𝑤 ∙ p (𝑘 ) 𝑖 =1 𝑖 𝑖 ( ) m 𝑘 = , (15) ∑ 𝑤 w p𝑖 (k + 1) 𝑖 =1 id(i) i=1 id(i) m(k + 1) = P . (16) 𝑛 w i=1 ∑ 𝑤 ∙ p (𝑘 + 1) 𝑖 =1 𝑖𝑑 (𝑖 ) (𝑖 ) m(𝑘 + 1) = . (16) Subsequently, we can move on to determine the∑ weighted covariance matrix, which will be used 𝑖 =1 𝑖 to calculate the innovation of rotation matrix and translation (in a given iteration of the algorithm) Subsequently, we can move on to determine the weighted covariance matrix, which will be used to calculate the innovation of rotation matrix and translation (in a given iteration of the algorithm) w p (k)p (k + 1) i=1 id(i) 𝑛 T C = P m(k)m (k + 1). (17) ∑ (𝑤 ∙ 𝐩 (𝑘 ) ∙ 𝐩 (𝑘 + 1)) 𝑖 =1 𝑖 𝑖 n ( ) id i w T (17) i=1 𝐂 = − m(𝑘 ) ∙ m (𝑘 + 1). 𝑖 =1 𝑖 Using the SVD decomposition method on the C matrix Point cloud k Delaunay triangulation Using the SVD decomposition method on the C matrix Point cloud k+1 Matched pairs C = USV . (18) 𝐂 = 𝐕 . (18) where matrices U, Σ, V are characteristic matrices for the SVD method. By means of decomposition where matrices U, S, V are characteristic matrices for the SVD method. By means of decomposition carried out on designated matrices, we obtain the innovations in given iteration— —of the R carried out on designated matrices, we obtain the innovations in given iteration—it—of the R rotation it rotation matrix and the 𝐓 translation: matrix and the T translation: it R = VU , (19) 𝐑 it = 𝐕 𝐔 , (19) T = m(k) R m(k + 1). (20) it it 4 ( ) ( ) 𝐓 = m 𝑘 − 𝐑 ∙ m 𝑘 + 1 . (20) The obtained values in the R and T matrices are partial values developed in a given iteration iter iter The obtained values in the 𝐑 and 𝐓 matrices are partial values developed in a given 𝑟𝑡𝑖𝑒 𝑟𝑡𝑖𝑒 of the algorithm. Based on this, the set m(k + 1) is translated at last iteration (see Figure 6) and as a iteration of the algorithm. Based on this, the set m(k + 1) is translated at last iteration (see Figure 6) result the translation and rotation is obtained: and as a result the translation and rotation is obtained: R(k + 1) = R :::R , (21) ( ) 𝐑 𝑘 + 1 = 𝐑 1 ∙ … ∙ 𝐑 itn , (21) 1 1 𝑡𝑖𝑛 T(k + 1) = (R T + T ) + ::: + (R T + T ). (22) Point cloud k 1,2,3... Pair’s number 𝐓 (𝑘 + 1) = (𝐑 it+1 ∙ 𝐓 it + 𝐓 it+1 ) + … + (𝐑 itn ∙ 𝐓 itn1 + 𝐓 itn ). (22) +1 𝒊𝒕 +𝟏 itn itn−1 itn Point cloud k Weighted pairs Point cloud k+1 where R and T are initialized values. Point cloud k+1 1,2,3... Pair’s number where 𝐑 it=1 and it 𝐓 =1 are initialized values. =1 =1 1,2,3... Pair’s number Point cloud k Point cloud k+1 Figure 6. Left side—2-point clouds; right side—2-point clouds aligned. Figure 6. Left side—2-point clouds; right side—2-point clouds aligned. " # Dx ∆𝑥 The translation vector T(k + 1) = thus obtained will be used to determine ( ) The translation vector 𝐓 𝑘 + 1 = [ ] thus obtained will be used to determine Dy ∆𝑦 2 2 √∆𝑥 +∆𝑦 2 2 (23) SOG(𝑘 + 1) = , Dx + Dy 𝜟 𝑡 SOG(k + 1) = , (23) Dt ( ) where ∆𝑡 is the distance in time between the moment 𝑘 of performing the measurements of ℤ(𝑘 ), and the moment (𝑘 + 1) of performing subsequent measurements of ℤ(𝑘 + 1). cos𝚫 𝜃 sin𝚫 𝜃 ( ) On the other hand, the final form of the rotation matrix 𝐑 𝑘 + 1 = [ ] will be −sin𝚫 𝜃 cos𝚫 𝜃 used to determine: 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝐔𝚺 𝑖𝑑 Appl. Sci. 2019, 9, 3530 9 of 17 where Dt is the distance in time between the moment (k) of performing the measurements of Z(k), and the moment (k + 1) of performing subsequent measurements of Z(k + 1). " # cosD sinD On the other hand, the ﬁnal form of the rotation matrix R(k + 1) = will be sinD cosD used Apto pl. determine: Sci. 2019, 9, x FOR PEER REVIEW 9 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 9 of 17 arc sin(D) ROT(k + 1) = , (24) Dt arc sin(∆𝜽 ) arc sin(∆𝜽 ) ( ) ROT 𝑘 + 1 = , (24) ROT(𝑘 + 1) = , (24) 𝚫 𝑡 𝚫 𝑡 and and and HDG(k + 1) = HDG + arc sin(D), (25) HDG(𝑘 + 1) = HDG + arc sin(𝚫 𝜃 ), (25) HDG(𝑘 + 1) = HDG 0 + arc sin(𝚫 𝜃 ), (25) where HDG is the initial HDG value determined using other methods (devices) prior to the commencement where HDG of is measur the initement. ial HDG value determined using other methods (devices) prior to the where HDG 0 is the initial HDG value determined using other methods (devices) prior to the commencement of measurement. commencement of measurement. 2.2. Weighted Matching Point Clouds Using the Mean Direction Error 2.2. Weighted Matching Point Clouds Using the Mean Direction Error 2.2. Weighted Matching Point Clouds Using the Mean Direction Error Figure 7 shows that the mean error value changes signiﬁcantly as the function of angle , and the Figure 7 shows that the mean error value changes significantly as the function of angle 𝛽 , and gradient of Figure these 7 changes shows thwil at th l incr e me ease an er while ror vathe lue eccentricity changes signof ifica the ntlellipse y as the isfu appr nctio oaching n of angl 1e (the 𝛽 , aellipse nd the gradient of these changes will increase while the eccentricity of the ellipse is approaching 1 (the the gradient of these changes will increase while the eccentricity of the ellipse is approaching 1 (the will be becoming ﬂat, which occurs where measurements are carried out using LIDAR). Owing to ellipse will be becoming flat, which occurs where measurements are carried out using LIDAR). ellipse will be becoming flat, which occurs where measurements are carried out using LIDAR). these particular properties, this error can usefully determine the value of translation and rotation Owing to these particular properties, this error can usefully determine the value of translation and Owing to these particular properties, this error can usefully determine the value of translation and established while minimalizing the error metric between the points from subsequent measurements rotation established while minimalizing the error metric between the points from subsequent rotation established while minimalizing the error metric between the points from subsequent (e.g., point clouds from LIDAR measurements). Figure 8 presents, in the graphical form, how the measurements (e.g., point clouds from LIDAR measurements). Figure 8 presents, in the graphical measurements (e.g., point clouds from LIDAR measurements). Figure 8 presents, in the graphical positions matched in pairs of points based on the corrections (e.g., those added to the components of form, how the positions matched in pairs of points based on the corrections (e.g., those added to the form, how the positions matched in pairs of points based on the corrections (e.g., those added to the the rotation matrix and translation vector) change; the value of the rotation and translation was made components of the rotation matrix and translation vector) change; the value of the rotation and components of the rotation matrix and translation vector) change; the value of the rotation and depending on the values of the sums of mean errors (Equation (9)) calculated along lines connecting the translation was made depending on the values of the sums of mean errors (Equation (9)) calculated translation was made depending on the values of the sums of mean errors (Equation (9)) calculated ( ) ( ) matched points from k and k + 1 cloud. In order to facilitate the interpretation of the phenomenon, along lines connecting the matched points from (𝑘 ) and (𝑘 + 1) cloud. In order to facilitate the along lines connecting the matched points from (𝑘 ) and (𝑘 + 1) cloud. In order to facilitate the it was inter assumed pretation that of th the e phen mean omerr eno or n, ellipses it was aswer sume ed similar that the inm size. ean error ellipses were similar in size. interpretation of the phenomenon, it was assumed that the mean error ellipses were similar in size. Figure Figure 7. A 7. grap A gra hic phic prpr esentation esentationof of the the corr corrected ected po position sition of of ththe e po points ints ma matche tched in d pa inir pai s urs sinusing g the the Figure 7. A graphic presentation of the corrected position of the points matched in pairs using the sums of mean errors calculated along the lines connecting the matched points. sums of mean errors calculated along the lines connecting the matched points. sums of mean errors calculated along the lines connecting the matched points. Figure 8. A graphic presentation the corrected position using the sums of mean errors CEP (circular Figure 8. A graphic presentation the corrected position using the sums of mean errors CEP (circular Figure 8. A graphic presentation the corrected position using the sums of mean errors CEP (circular error probable) calculated for the matched points. error probable) calculated for the matched points. error probable) calculated for the matched points. As can be seen in Figure 7, greater weight is introduced for the points whose mean errors in the As can be seen in Figure 7, greater weight is introduced for the points whose mean errors in the specified direction (point-to-point direction) is lower–pairs marked as 2 and 3 in Figure 7. The heavier specified direction (point-to-point direction) is lower–pairs marked as 2 and 3 in Figure 7. The heavier Appl. Sci. 2019, 9, 3530 10 of 17 As can be seen in Figure 7, greater weight is introduced for the points whose mean errors in the Appl. Sci. 2019, 9, x FOR PEER REVIEW 10 of 17 speciﬁed direction (point-to-point direction) is lower–pairs marked as 2 and 3 in Figure 7. The heavier weight influences the rotation and translation more and that is why pair 2 and 3 are better aligned weight inﬂuences the rotation and translation more and that is why pair 2 and 3 are better aligned comparing with 1 and 4. The use of such a weighting coefficient allows taking into account the spatial comparing with 1 and 4. The use of such a weighting coecient allows taking into account the spatial distribution of errors according to the selected direction. For comparison, Figure 8 presents, in a distribution of errors according to the selected direction. For comparison, Figure 8 presents, in a graphical form, how the positions matched in pairs of points based on the corrections whose values graphical form, how the positions matched in pairs of points based on the corrections whose values depended on the values of the sums of mean errors (Equation (5)) calculated for the matched points. depended on the values of the sums of mean errors (Equation (5)) calculated for the matched points. As can be seen each pair (1, 2, 3, 4) influences the rotation and translation in similar manner. As can be seen each pair (1, 2, 3, 4) inﬂuences the rotation and translation in similar manner. HDG, SOG, and ROT were determined by processing LIDAR scans archived in data packets in HDG, SOG, and ROT were determined by processing LIDAR scans archived in data packets in accordance with the methodology presented in Section 2.1. The pseudo-code of the program used for accordance with the methodology presented in Section 2.1. The pseudo-code of the program used for the calculations using the authors original weighting factor 𝑤 (𝑘 , 𝑘 + 1) and Huber’s robust function ( ) the calculations using the authors original weighting factor w k, k + 1 and Huber ’s robust function coefficient 𝑘𝐻𝑢 = 𝑐 ∙ Median ‖𝐩 (𝑘 ) − 𝐩 (𝑘 + 1)‖ finally took the following form: coecient kHu = cMediankp (k) p (k + 1)k ﬁnally took the following form: 1,…,𝑛 (1,…𝑛 ) 1,:::,n id(1,:::n) A Algorithm lgorithm 1 1 The Theweighted weighteICP d ICP 𝑰𝒏𝒑𝒖𝒕 : 𝐌 (𝑘 ), 𝐌 (𝑘 + 1), , , 𝐑 = 𝐈 , 𝐓 = 0, 𝑑 = 0, 𝑡 ℎ𝑒𝑟𝑠 ℎ𝑜𝑙𝑑 𝑟 𝛼 𝑠𝑟𝑒 𝑓 𝑜𝑟 𝑖 ← 1 𝑡𝑜 𝑛 ( ) |𝑖𝑛𝑑𝑓 𝑖𝑑 𝑖 = argmin ‖𝐑 𝐩 (𝑘 ) + 𝐓 − 𝐩 (𝑘 + 1)‖ ; 𝑖 𝑖𝑑 (𝑖 ) 𝑖 ∈{1,..,𝑛 } 𝑑𝑒𝑛 ( ) ( ) 𝐻𝑢𝑘 = 𝑐 ∙ Median ‖𝐩 𝑘 − 𝐩 𝑘 + 1 ‖, 𝑐 = 2.0138; 1,…,𝑛 𝑖𝑑 (1,…𝑛 ) 𝑓 𝑜𝑟 𝑖 ← 1 𝑡𝑜 𝑛 − 0.5 2 2 | 𝑤 = (𝜎 (𝑘 ) + 𝜎 (𝑘 + 1)) ; 𝑖 𝛽 𝛽 𝑖 𝑖𝑑 (𝑖 ) 𝑜𝑟 − 0.5 2 2 𝑤 = ((𝑀 (𝑘 ) + 𝑀 (𝑘 + 1))) ; 𝑖 𝑖 𝑖𝑑 (𝑖 ) 𝑑𝑒𝑛 𝑖𝑓 ‖𝐩 (𝑘 ) − 𝐩 (𝑘 + 1)‖ > 𝐻𝑢𝑘 𝑖 𝑖𝑑 (𝑖 ) 𝐻𝑢𝑘 |𝑤 = ∙ 𝑤 ; 𝑖 𝑖 ‖𝐩 (𝑘 ) − 𝐩 (𝑘 + 1)‖ 𝑖 𝑖𝑑 (𝑖 ) [𝐑 , 𝐓 ] ← argmin∑ 𝑤 ‖𝐑 𝐩 (𝑘 ) + 𝐓 − 𝐩 (𝑘 + 1)‖ ; 𝑖 𝑖 𝑖𝑑 (𝑖 ) 𝐑 ,𝐓 𝑖 =1 𝑜𝑟𝑓 𝑖 ← 1 𝑡𝑜 𝑛 |𝐩 (𝒌 ) = 𝐑 ∙ 𝐩 (𝒌 ) + 𝐓 ; 𝑖 𝑖 | 𝑑𝑒𝑛 𝑑 (𝐩 (𝑘 ) − 𝐩 (𝑘 + 1)) 𝑖 id(𝑖 ) 𝑤 = ∑ ; 𝑟𝑠𝑒 𝑖 =1 𝑖𝑓𝑓𝑑 = 𝑤 − 𝑜𝑙 𝑑 ; 𝑟𝑠𝑒 𝑟𝑠𝑒 𝑜𝑙 𝑑 = 𝑤 ; 𝑟𝑠𝑒 𝑟𝑠𝑒 𝑤 ℎ𝑖𝑙𝑒 (𝑖𝑓𝑓𝑑 ≥ 𝑡 ℎ𝑟𝑒𝑠 ℎ𝑜𝑙𝑑 ) 3. The Study and the Analysis of the Obtained Results 3. The Study and the Analysis of the Obtained Results The main aim of the study was to evaluate the accuracy of the determination of the HDG, SOG and ROT using a modiﬁed ICP algorithm, carried out as a result of their comparison with high-accuracy The main aim of the study was to evaluate the accuracy of the determination of the HDG, SOG reference measurements. The study involved: and ROT using a modified ICP algorithm, carried out as a result of their comparison with high- accuracy reference measurements. The study involved: • measurements carried out using LIDAR and a GNSS RTK receiver, and the synchronous recording of their results, 𝑛𝑒 𝑛𝑒 𝑛𝑒 𝑛𝑑 𝑑𝑜 𝑜𝑙 𝑖𝑑 Appl. Sci. 2019, 9, 3530 11 of 17 measurements carried out using LIDAR and a GNSS RTK receiver, and the synchronous recording of their results, Appl. Sci. 2019, 9, x FOR PEER REVIEW 11 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 11 of 17 the determination of the HDG, SOG and ROT, based on LIDAR measurement results, by the scan-matching method using both classic and modiﬁed versions of the ICP algorithm, • the determination of the HDG, SOG and ROT, based on LIDAR measurement results, by the • the determination of the HDG, SOG and ROT, based on LIDAR measurement results, by the an analysis comparing the HDG, SOG, and ROT with reference results of GNSS RTK measurements, sca scan n- -m ma atchi tchin ng g m metho ethod d usi usin ng b g bo oth th cl cla ass ssiic c a an nd d m mo od diif fiied ed v ver ersi sio on ns o s of f th the e ICP ICP a allgo gori rit th hm m,, based on the accuracy criterion. •• a an n a an na allys ysiis co s com mpa pari rin ng t g th he e HD HDG G,, S SO OG G,, a an nd d R ROT wi OT with th re ref fer erence ence re resul sults ts o of f G GNS NSS S R RTK TK measurements, based on the accuracy criterion. measurements, based on the accuracy criterion. The yacht port basin in Gdynia (Poland) was selected for the measurement area. Its boundaries include high, concrete wharves and a breakwater. Most of the time, good hydro-meteorological The yacht port basin in Gdynia (Poland) was selected for the measurement area. Its boundaries The yacht port basin in Gdynia (Poland) was selected for the measurement area. Its boundaries conditions prevail in the area (Figure 9). To carry out the measurements, a small USV (with a length iin ncl clude ude h hiigh gh, , co con ncr crete ete wh wha ar rv ves es a an nd d a a b bre rea akw kwa ater ter.. M Mo ost st o of f th the e ti tim me, e, go goo od d h hydr ydro o- -m mete eteo orol rolo ogi gica cal l of 1.62 cond m, itio width ns prev of ail0.40 in thm, e are and a (Fi agure draught 9). Toof carr 0.11 y out m) th equipped e measureme with nts,Scanse a small Sweep USV (wi Lidar th a l,engt a GNSS h conditions prevail in the area (Figure 9). To carry out the measurements, a small USV (with a length RTKo or f f eceiver 1 1..6 62 2 m m,, wi wi Leica d dth th o V of fiva 0 0..4 40 0 CS m m,15 , a an n[d d 48 a a], d dra and rau ugh gh an t t o oon-boar f f 0 0..1 11 1 m m) ) d equ equ computer iippe pped d wi wi con th th S S nected ca can nse se S Sto wee wee them p p L Liid d with a ar, r, a a G G a NS NS RS-422 S S R RTK TK /232C re rece ceiiv ver er L Le eiica ca Vi Viv va a CS CS 1 15 5 [ [4 48 8] ],, a an nd d a an n o on n- -b bo oa ard rd co com mpute puter r co con nn nec ected ted to to th them em wi with th a a R RS S- -4 42 22 2//2 23 32 2C C conversion cable [49], were used to record the measurement results (Figure 10). The measurements conversion cable [49], were used to record the measurement results (Figure 10). The measurements conversion cable [49], were used to record the measurement results (Figure 10). The measurements were carried out on a USV sailing with a speed of approximately 1.2 (2.4 kts) over a trajectory m m were carried out on a USV sailing with a speed of approximately 1.2 ( (2.4 kts) ) over a trajectory of were carried out on a USV sailing with a speed of approximately 1.2 2.4 kts over a trajectory of of approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer s s approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer wirelessly connected to AUSV (autonomous unmanned surface vehicle, see Figure 11.). While the wi wire rellessl essly y c co on nn nec ected ted to to A AU US SV V ( (a auto uton no om mo ous us u un nm ma an nn ned ed surf surfa ace ce v ve eh hiicl cle e,, see see F Fig igure ure 11 11..) ).. Wh Whiille e th the e USV was sailing, the on-board computer synchronously recorded (when generating a full scan using USV was sailing, the on-board computer synchronously recorded (when generating a full scan using USV was sailing, the on-board computer synchronously recorded (when generating a full scan using LIDAR) the measurement results making up the so-called data sets containing: a LIDAR scan (in the L LIDAR) IDAR) th the e m me ea as sure urem men ent r t resul esults ts m ma aki kin ng up g up th the e so so- -ca calllled ed d da ata ta sets co sets con nta taiin niin ng: g: a a L LID IDA AR R sc sca an n ( (iin n th the e form of a measurement set Z = (r , ), ::: , (r , ) ), NMEA messages with the position coordinates n n 1 1 f fo orm rm o of f a a m mea easure surem ment set ent set ℤ ℤ = = { {( (𝑟 𝑟 ,, ) ),, … … ,, ( (𝑟 𝑟 ,, ) )} }) ),, NM NME EA A m messa essage ges s wi with th t th he e po posi sit tiio on n coo coord rdiin na ates tes 1 1 1 1 n n n n of SOG and COG (course over ground) from the GNSS RTK receiver [50]. 865 data sets were thus o of f S SOG OG a an nd d COG COG ( (c co ourse urse o ov ver er g gro roun und d) ) f fro rom m th the e G GNS NSS S R RTK TK re rec cei eiv ver er [ [5 50 0] ].. 8 86 65 5 d da at ta a sets sets were were th thu us s collected by recording each subsequent sets every 0.2 s on average, i.e., after a change in the USV co coll llec ected ted b by y re reco cord rdiin ng g ea each ch su subseq bseque uen nt t se sets ts e ev ver ery y 0 0..2 2 s s o on n a av ver era age, ge, ii..e. e.,, a af fter ter a a c ch ha an nge ge iin n th the e U US SV V position by approx. 0.25 m. position by approx. 0.25 m. position by approx. 0.25 m. Figure 9. The trajectory covered by the USV at the time of measurements. Figure 9. The trajectory covered by the USV at the time of measurements. Figure 9. The trajectory covered by the USV at the time of measurements. Fig Figure ure 1 10 0. . Th The e di dist str rib ibut utio ion n o of f m mea easur surin ing g in inst str rum umen entts s o on n tth he e unma unman nn ned ed sur surfa fac ce e v ve eh hicle icle ( (USV USV) ).. Figure 10. The distribution of measuring instruments on the unmanned surface vehicle (USV). Appl. Sci. 2019, 9, x FOR PEER REVIEW 12 of 17 Appl. Sci. 2019, 9, 3530 12 of 17 (a) (b) Figure 11. (a) USV during a collection data phase; (b) wireless control system. Figure 11. (a) USV during a collection data phase; (b) wireless control system. Analysis of the Accuracy of Determining the COG and SOG Using a Modiﬁed ICP Algorithm Analysis of the Accuracy of Determining the COG and SOG Using a Modified ICP Algorithm Table 1 presents statistical parameters (i.e., the mean value, standard deviation and the maximum Table 1 presents statistical parameters (i.e., the mean value, standard deviation and the value) which characterize the computational accuracy of each parameter. The following acronyms maximum value) which characterize the computational accuracy of each parameter. The following were used to describe ICP’s variants: K_ICP for algorithm without modiﬁcation, H_ICP for robust acronyms were used to describe ICP’s variants: K_ICP for algorithm without modification, H_ICP version based on Huber ’s function, HM_ICP for robust version based on Huber ’s function with mean for robust version based on Huber’s function, HM_ICP for robust version based on Huber’s function error weighting and HD_ICP for robust version based on Huber ’s function with directional error with mean error weighting and HD_ICP for robust version based on Huber’s function with weighting. The lower index ICP was used for values computed by ICP algorithm’s variants and lower directional error weighting. The lower index ICP was used for values computed by ICP algorithm’s index R was introduced to point the values surveyed with GNSS RTK receiver. variants and lower index R was introduced to point the values surveyed with GNSS RTK receiver. Table 1. Summary of comparative statistical parameters. Table 1. Summary of comparative statistical parameters. Mean Value Standard Deviation Maximum Value Mean Value Standard Deviation Maximum Value Method SOG SOG DCOG DCOG SOG SOG DCOG DCOG SOG SOG DCOG DCOG R R R R R R ICP ICP ICP ICP ICP ICP 𝐒𝐎𝐆 ∆𝐂𝐎𝐆 𝐒𝐎 𝐆 ∆𝐂𝐎𝐆 𝐒𝐎𝐆 ∆𝐂𝐎𝐆 (m/s) ( ) (m/s) ( ) (m/s) ( ) Method − 𝐒𝐎𝐆 − ∆𝐂𝐎𝐆 − 𝐒𝐎𝐆 − ∆𝐂𝐎𝐆 − 𝐒𝐎𝐆 − ∆𝐂𝐎𝐆 𝐑 𝐑 𝐑 𝐑 𝐑 𝐑 K_ICP 0.031 0.08 0.113 0.57 0.524 3.5 (m/s) (°) (m/s) (°) (m/s) (°) H_ICP 0.013 0.05 0.031 0.14 0.319 0.72 HM_ICP 0.012 0.03 0.031 0.13 0.185 0.48 K_ICP −0.031 0.08 0.113 0.57 −0.524 −3.5 HD_ICP 0.007 0.03 0.026 0.11 0.18 0.42 H_ICP −0.013 0.05 0.031 0.14 −0.319 0.72 HM_ICP −0.012 0.03 0.031 0.13 −0.185 −0.48 A comparison of the parameter values (obtained based on 865 collected data packets) listed in HD_ICP −0.007 0.03 0.026 0.11 −0.18 0.42 Table 1 shows that the HD_ICP algorithm was the best in terms of the accuracy of determining the USVs movement parameters; it is followed by HM_ICP, H_ICP and K_ICP. Figure 12 shows a graph of A comparison of the parameter values (obtained based on 865 collected data packets) listed in SOG_ICP-SOG_R dierences. Table 1 shows that the HD_ICP algorithm was the best in terms of the accuracy of determining the Figure 12 shows that the accuracy of determining the SOG using the K_ICP method signiﬁcantly USVs movement parameters; it is followed by HM_ICP, H_ICP and K_ICP. Figure 12 shows a graph decreases for scans in the intervals ofh220, 365i andh720, 825i. These scans dier signiﬁcantly from of SOG_ICP-SOG_R differences. each other in rotation angles and include a greater number of erroneous measurement results than the others. This is undoubtedly due to the turns made by the USV at that time. The accuracy of determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber ’s function. The HD_ICP method proved to be the best in determining the SOG in a signiﬁcant part of the graph course. This was also conﬁrmed by the lowest values of the mean error, equal to0.007 (0.014 kts) ( ) and of the standard deviation, equal to 0.026 0.05 kts presented in Table 1. 𝐈𝐂𝐏 𝐈𝐂𝐏 𝐈𝐂𝐏 𝐈𝐂𝐏 𝐈𝐂𝐏 𝐈𝐂𝐏 Appl. Sci. 2019, 9, x FOR PEER REVIEW 13 of 17 Appl. Sci. 2019, 9, 3530 13 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 13 of 17 Figure 12. A graph of SOG − SOG differences. ICP R Figure 12 shows that the accuracy of determining the SOG using the K_ICP method significantly decreases for scans in the intervals of ⟨220, 365⟩ and ⟨720, 825⟩. These scans differ significantly from each other in rotation angles and include a greater number of erroneous measurement results than the others. This is undoubtedly due to the turns made by the USV at that time. The accuracy of determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber’s function. The HD_ICP method proved to be the best in determining the SOG in a significant part of the graph course. This was also confirmed by the lowest values of the mean error, equal to m m −0.007 (−0.014 kts) and of the standard deviation, equal to 0.026 (0.05 kts) presented in Table Figure 12. A graph of SOG SOG dierences. s s ICP R Figure 12. A graph of SOG − SOG differences. ICP R The Figure histogram 12 shows repr tha esent t the ing accthe uracy frequency of determi of occurr ning th ence e SOG of SOG using th e SOG K_ICP di m et er hences, od sign pr if esented icantly The histogram representing the frequency of occurrence ICP of SOG R − SOG differences, ICP R in dec Figur reases e 13 fo ,r shows scans even in the mor inter e clearly vals of that ⟨220 in , 365 terms ⟩ aof nd the ⟨720 accuracy , 825⟩. of These determining scans difthe fer s SOG, ignifithe canHD_ICP tly from presented in Figure 13, shows even more clearly that in terms of the accuracy of determining the method each othis erbetter in rota than tionthe angl others. es andMost include of the a gre values ater of num SOG ber of er SOG roneodi us m erea ences, surement determined resultsusing than SOG, the HD_ICP method is better than the others. Most of ICP the values Rof SOG − SOG differences, ICP R the the HD_ICP others. T , HM_ICP his is un,d and oubte H_ICP dly dmethods, ue to the fall turn within s madthe e by interval the US of V a0.05~0.05. t that time. The accuracy of determined using the HD_ICP, HM_ICP, and H_ICP methods, fall within the interval of −0.05~0.05. determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber’s function. The HD_ICP method proved to be the best in determining the SOG in a significant part of the graph course. This was also confirmed by the lowest values of the mean error, equal to m m −0.007 (−0.014 kts) and of the standard deviation, equal to 0.026 (0.05 kts) presented in Table s s The histogram representing the frequency of occurrence of SOG − SOG differences, ICP R presented in Figure 13, shows even more clearly that in terms of the accuracy of determining the SOG, the HD_ICP method is better than the others. Most of the values of SOG − SOG differences, ICP R determined using the HD_ICP, HM_ICP, and H_ICP methods, fall within the interval of −0.05~0.05. Figure Figure 13. 13. H Histogram istogram r repr epre esenting senting t the he fr frequency equency of of occurr occurrence ence of of SOG SOG − SOG SOG di differ ere ences. nces. I ICP CP R R Figure 14 shows that the accuracy of determination of the DCOG using the K_ICP method Figure 14 shows that the accuracy of determination of the ΔCOG using the K_ICP method signiﬁcantly decreases, as in the case of the SOG, for scans in the intervals ofh220, 365i andh720, 825i. significantly decreases, as in the case of the SOG, for scans in the intervals of ⟨220, 365⟩ and The HD_ICP method also proved to be the best in determining the DCOG in this case. This was also ⟨720, 825⟩. The HD_ICP method also proved to be the best in determining the ΔCOG in this case. This conﬁrmed by the lowest values of the mean error, equal to 0.03 , and of the standard deviation, equal was also confirmed by the lowest values of the mean error, equal to 0.03°, and of the standard to 0.11 , presented in Table 1. Figure 15 shows a histogram representing the frequency of occurrence of deviation, equal to 0.11° , presented in Table 1. Figure 15 shows a histogram representing the DCOG DCOG increment dierences. frequency of occurrence of ΔCOG − ΔCOG increment differences. ICP R ICP R The histogram representing the frequency of the occurrence of dierences DCOG DCOG ICP R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the DCOG. Its advantage over the others can be clearly seen in the interval Figure 13. Histogram representing the frequency of occurrence of SOG − SOG differences. ICP R of dierences DCOG DCOG h0.05, 0.05i. Given that the DROT results are identical with the ICP R DCOG results, their presentation and analysis have been omitted. Dierences in the algorithms’ Figure 14 shows that the accuracy of determination of the ΔCOG using the K_ICP method accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Dierences between significantly decreases, as in the case of the SOG, for scans in the intervals of ⟨220, 365⟩ and HD_ICP and HM_ICP versions are barely visible, so they were omitted. ⟨ ⟩ 720, 825 . The HD_ICP method also proved to be the best in determining the ΔCOG in this case. This was also confirmed by the lowest values of the mean error, equal to 0.03°, and of the standard deviation, equal to 0.11° , presented in Table 1. Figure 15 shows a histogram representing the frequency of occurrence of ΔCOG − ΔCOG increment differences. ICP R 0.50 — 0.55 0.50 — 0.55 0.50 — 0.55 0.45 — 0.50 0.45 — 0.50 0.45 — 0.50 0.40 — 0.45 0.40 — 0.45 0.40 — 0.45 0.35 — 0.40 0.35 — 0.40 0.35 — 0.40 0.30 — 0.35 0.30 — 0.35 0.30 — 0.35 0.25 — 0.30 0.25 — 0.30 0.25 — 0.30 0.20 — 0.25 0.20 — 0.25 0.20 — 0.25 0.15 — 0.20 0.15 — 0.20 0.15 — 0.20 0.10 — 0.15 0.10 — 0.15 0.10 — 0.15 0.05 — 0.10 0.05 — 0.10 0.05 — 0.10 -0.05 — 0.05 -0.05 — 0.05 -0.05 — 0.05 -0.10 — -0.05 -0.10 — -0.05 -0.10 — -0.05 -0.15 — -0.10 -0.15 — -0.10 -0.15 — -0.10 -0.20 — -0.15 -0.20 — -0.15 -0.20 — -0.15 -0.25 — -0.20 -0.25 — -0.20 -0.25 — -0.20 -0.30 — -0.25 -0.30 — -0.25 -0.30 — -0.25 -0.35 — -0.30 -0.35 — -0.30 -0.35 — -0.30 -0.40 — -0.35 -0.40 — -0.35 -0.40 — -0.35 -0.45 — -0.40 -0.45 — -0.40 -0.45 — -0.40 -0.50 — -0.45 -0.50 — -0.45 -0.50 — -0.45 Appl. Sci. 2019, 9, x FOR PEER REVIEW 14 of 17 2.0 K_ICP H_ICP HM_ICP HD_ICP 1.0 0.0 -1.0 Appl. Sci. 2019, 9, 3530 14 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 14 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 14 of 17 -2.0 2.0 2.0 -3.0 K_ICP H_ICP HM_ICP HD_ICP K_ICP H_ICP HM_ICP HD_ICP 1.0 1.0 -4.0 1 50 100 150 200 250 300 350 400 450 500 550 600 650 700 750 800 850 Scan number 0.0 0.0 Figure 14. A graph of ΔCOG − ΔCOG increment differences. ICP R -1.0 -1.0 -2.0 1 -2 6.0 0 -3.0 1 -3 2.0 0 K_ICP H_ICP HM_ICP HD_ICP -4.0 -4.0 1 50 100 150 200 250 300 350 400 450 500 550 600 650 700 750 800 850 1 50 100 150 200 250 300 350 400 450 500 550 600 650 700 750 800 850 Scan number Scan number Figure 14. A graph of ΔCOG − ΔCOG increment differences. Figure 14. A graph of ΔCOG ICP − ΔCOG R increment differences. Figure 14. A graph of DCOG DCOG increment dierences. IICP CP R R Δ COG difference [º] K_ICP H_ICP HM_ICP HD_ICP K_ICP H_ICP HM_ICP HD_ICP Figure 15 60 . A histogram representing the frequency of occurrence of ΔCOG − ΔCOG increments. 60 ICP R The histogram representing the frequency of the occurrence of differences ΔCOG − ΔCOG ICP R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval ⟨ ⟩ of differences ΔCOG − ΔCOG −0.05˚, 0.05˚ . Given that the ΔROT results are identical with the ICP R ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms’ Δ COG difference [º] Δ COG difference [º] accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between Figure 15. A histogram representing the frequency of occurrence of ΔCOG − ΔCOG increments. HD_ICP Figure and 15 HM . A _ hICP istogv ra er m si ro epr ns a esen re tb in ag re th ly e v friequen sible, cso y o th f o ey ccurr were enc e om of itted ΔCOG . ICP − ΔCOG R increments. Figure 15. A histogram representing the frequency of occurrence of DCOGICP DCOG R increments. ICP R The histogram representing the frequency of the occurrence of differences ΔCOG − ΔCOG The histogram representing the frequency of the occurrence of differences ΔCOG ICP − ΔCOG R ICP R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval ⟨ ⟩ of differences ΔCOG − ΔCOG −0.05˚, 0.05˚ . Given that the ΔROT results are identical with the ⟨ ⟩ of differences ΔCOG ICP − ΔCOG R −0.05˚, 0.05˚ . Given that the ΔROT results are identical with the ICP R ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms’ ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms’ accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between HD_ICP and HM_ICP versions are barely visible, so they were omitted. HD_ICP and HM_ICP versions are barely visible, so they were omitted. (a) (b) Figure 16. (a) Grid map computed by ICP with proposed directional weighting—HD_ICP; (b) grid map computed standard H_ICP. 4. Conclusions As the study attempted to prove, an ICP algorithm that matches point clouds from a laser scanner can eectively generate USV movement parameters. The proposed weighting factor based on the mean error in the speciﬁed direction of the determination of the coordinates of the positions of (a) (b) (a) (b) Δ COG difference [º] Number of samp Δ le COG s difference [º] Number of samples Δ COG difference [º] Number of samples Appl. Sci. 2019, 9, 3530 15 of 17 scan points enables more realistic (corresponding to the actual movement) matching of subsequent maps of the surrounding environment. This helps to determine the USV movement parameters with a higher accuracy. This was conﬁrmed by the study results—inter alia the values of statistical indices in the form of mean SOG SOG = 0.007 (0.014 kts) and standard deviation ICP R SOG SOG = 0.0025 (0.05 kts) and DCOG DCOG = 0.03 and standard deviation ICP R ICP R DCOG DCOG = 0.11 . However, it should be noted that the authors’ original weighting factor ICP R should be used in combination with a robust criterion function to reduce measurements with gross errors (e.g., based on Huber ’s function), as only such a solution can be fully applicable. The indicated mean error values lead us to a generalized statement that the developed method allows measuring SOG and COG with the accuracy required by the IMO. It should be noted, however, that the test was carried out in conﬁned waters (harbors, channels, berths, marinas), beyond which the method would not be eective due to the low LIDAR range. All data used in publication is available at [51]. Author Contributions: Ł.M., methodology, investigation, writing-original draft preparation, writing-review and editing, K.N., supervision, formal analysis, validation and funding acquisition. Funding: The article has been funded by a statutory research on marine optical navigation systems conducted in Polish Naval Academy. Conﬂicts of Interest: The authors declare no conﬂict of interest. References 1. IMO. Resolution A.824(19) Adoption of Amendments to Performance Standards for Devices to Measure and Indicate Speed and Distance; IMO: London, UK, 2000. 2. IMO. Resolution MSC.116(73), Performance Standards for Marine Transmitting Heading Devices (thds); IMO: London, UK, 2000. 3. Naus, K.; Waz, M. Precision in Determining Ship Position using the Method of Comparing an Omnidirectional Map to a Visual Shoreline Image. J. Navig. 2016, 69, 391–413. [CrossRef] 4. Polvara, R.; Sharma, S.; Wan, J.; Manning, A.; Sutton, R. Obstacle Avoidance Approaches for Autonomous Navigation of Unmanned Surface Vehicles. J. Navig. 2018, 71, 241–256. [CrossRef] 5. Allen, C.H. The Seabots are Coming Here: Should they be Treated as ‘Vessels’? J. Navig. 2012, 65, 749–752. [CrossRef] 6. Nowak, A. The Proposal to “Snapshot” Raim Method for Gnss Vessel Receivers Working in Poor Space Segment Geometry. Pol. Marit. Res. 2015, 22, 3–8. [CrossRef] 7. Xie, P.; Petovello, M.G. Measuring GNSS Multipath Distributions in Urban Canyon Environments. IEEE Trans. Instrum. Meas. 2015, 64, 366–377. 8. Wang, Y.; Chen, X.; Liu, P. Statistical Multipath Model Based on Experimental GNSS Data in Static Urban Canyon Environment. Sensors 2018, 18, 1149. [CrossRef] 9. Shaﬁee, E.; Mosavi, M.R.; Moazedi, M. Detection of Spooﬁng Attack using Machine Learning based on Multi-Layer Neural Network in Single-Frequency GPS Receivers. J. Navig. 2018, 71, 169–188. [CrossRef] 10. Guo, R.; Sun, F.; Yuan, J. ICP based on Polar Point Matching with application to Graph-SLAM. In Proceedings of the 2009 International Conference on Mechatronics and Automation, Changchun, China, 9–12 August 2009; pp. 1122–1127. 11. Navarro, W.; Vélez, J.C.; Orﬁla, A. Estimation of sea state parameters using X-band marine radar technology in coastal areas. In Proceedings of the Sixth International Conference on Remote Sensing and Geoinformation of the Environment (RSCy2018), Paphos, Cyprus, 26–29 March 2018. 12. Mendes, E.; Koch, P.; Lacroix, S. ICP-based pose-graph SLAM. In Proceedings of the 2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), Lausanne, Switzerland, 23–27 October 2016; pp. 195–200. 13. Yang, J.; Li, H.; Jia, Y. Go-ICP: Solving 3D Registration Eciently and Globally Optimally. In Proceedings of the 2013 IEEE International Conference on Computer Vision, Sydney, Australia, 1–8 December 2013; pp. 1457–1464. 14. Zhang, Z. Iterative point matching for registration of free-form curves and surfaces. Int. J. Comput. Vis. 1994, 13, 119–152. [CrossRef] Appl. Sci. 2019, 9, 3530 16 of 17 15. Men, H.; Gebre, B.; Pochiraju, K. Color point cloud registration with 4D ICP algorithm. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 1511–1516. 16. Durrant-Whyte, H.; Bailey, T. Simultaneous localization and mapping: Part I. IEEE Robot. Autom. Mag. 2006, 13, 99–110. [CrossRef] 17. Moratuwage, M.D.P.; Wijesoma, W.S.; Kalyan, B.; Dong, J.F.; Namal Senarathne, P.G.C.; Hover, F.S.; Patrikalakis, N.M. Collaborative multi-vehicle localization and mapping in marine environments. In Proceedings of the OCEANS’10 IEEE SYDNEY, Sydney, Australia, 24–27 May 2010; pp. 1–6. 18. Ribas, D.; Ridao, P.; Neira, J. Underwater SLAM for Structured Environments Using an Imaging Sonar; Springer Tracts in Advanced Robotics Springer Berlin Heidelberg: Berlin/Heidelberg, Germany, 2010; ISBN 978-3-642-14039-6. 19. Nieto, J.; Bailey, T.; Nebot, E. Recursive scan-matching SLAM. Robot. Auton. Syst. 2007, 55, 39–49. [CrossRef] 20. Pomerleau, F.; Colas, F.; Siegwart, R.; Magnenat, S. Comparing ICP variants on real-world data sets: Open-source library and experimental protocol. Auton. Robot. 2013, 34, 133–148. [CrossRef] 21. Rusinkiewicz, S.; Levoy, M. Ecient variants of the ICP algorithm. In Proceedings of the Third International Conference on 3-D Digital Imaging and Modeling, Quebec City, QC, Canada, 28 May–1 June 2001; pp. 145–152. 22. Olson, E. Robust and Ecient Robotic Mapping. Ph.D. Thesis, Department of Electrical Engineering and Computer Science, MIT, Cambridge, MA, USA, 2008. 23. Konecny, J.; Prauzek, M.; Kromer, P.; Musilek, P. Novel Point-to-Point Scan Matching Algorithm Based on Cross-Correlation. Mob. Inf. Syst. 2016, 2016, 1–11. [CrossRef] 24. Besl, P.J.; McKay, N.D. A method for registration of 3-D shapes. IEEE Trans. Pattern Anal. Mach. Intell. 1992, 14, 239–256. [CrossRef] 25. Callmer, J.; Törnqvist, D.; Gustafsson, F.; Svensson, H.; Carlbom, P. Radar SLAM using visual features. EURASIP J. Adv. Signal Process. 2011, 2011, 71–82. [CrossRef] 26. Amigoni, F.; Gasparini, S.; Gimi, M. Scan matching without odometry information. In Proceedings of the First International Conference on Informatics in Control, Automation and Robotics, SciTePress—Science and Technology Publications, Setúbal, Portugal, 25–28 August 2004; pp. 349–352. 27. He, Y.; Liang, B.; Yang, J.; Li, S.; He, J. An Iterative Closest Points Algorithm for Registration of 3D Laser Scanner Point Clouds with Geometric Features. Sensors 2017, 17, 1862. [CrossRef] [PubMed] 28. Censi, A. An ICP variant using a point-to-line metric. In Proceedings of the 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, USA, 19–23 May 2008; pp. 19–25. 29. Segal, A.; Haehnel, D.; Thrun, S. Generalized-ICP. In Proceedings of the Robotics: Science and Systems V; Robotics: Science and Systems Foundation, Boston, MA, USA, 28 June–01 July 2009. 30. Wang, Y.; Xiong, R.; Li, Q. Em-based point to plane icp for 3d simultaneous localization and mapping. Int. J. Robot. Autom. 2013, 28. [CrossRef] 31. Godin, G.; Rioux, M.; Baribeau, R. Three-Dimensional Registration Using Range and Intensity Information; The International Society for Optical Engineering: Boston, MA, USA, 1994. 32. Marinov, A.; Zlateva, N.; Dimov, D.; Marinov, D. Weighted ICP Algorithm for Alignment of Stars from Scanned Astronomical Photographic Plates. Serdica J. Comput. Soﬁa Bulg. 2012, 6, 101–110. 33. Amamra, A.; Aouf, N.; Stuart, D.; Richardson, M. A recursive robust ﬁltering approach for 3D registration. SignalImage Video Process. 2016, 10, 835–842. [CrossRef] 34. Wang, R.; Geng, Z. WA-ICP algorithm for tackling ambiguous correspondence. In Proceedings of the 2015 3rd IAPR Asian Conference on Pattern Recognition (ACPR), Kuala Lumpur, Malaysia, 3–6 November 2015; pp. 76–80. 35. Naus, K.; Wa˛z, ˙ M. Ripping Radar Image with the Sea Map Using the Alignment Method 2009–2019; Logistyka, Institute of Logistics and Warehousing: Poznan, Poland, 2010. 36. Naus, K.; Wa˛z, ˙ M. Accuracy in ﬁxing ship’s positions by camera survey of bearings. Geod. Cartogr. 2011, 60, 61–73. [CrossRef] 37. Scansce. User ’s Manual and Technical Speciﬁcations. Available online: www.robotshop.com/media/ﬁles/ pdf2/sweep_user_manual.pdf (accessed on 1 February 2019). 38. Marden, S.; Guivant, J. Improving the Performance of ICP for Real-Time Applications using an Approximate Nearest Neighbour Search. In Proceedings of the Australasian Conference on Robotics and Automation, Wellington, New Zealand, 3–5 December 2012; pp. 3–5. Appl. Sci. 2019, 9, 3530 17 of 17 39. Bentley, J.L. Multidimensional binary search trees used for associative searching. Commun. ACM 1975, 18, 509–517. [CrossRef] 40. Greenspan, M.; Yurick, M. Approximate K-D tree search for ecient ICP. In Proceedings of the Fourth International Conference on 3-D Digital Imaging and Modeling, Ban, AB, Canada, 6–10 October 2003; pp. 442–448. 41. Jost, T.; Hugli, H. A multi-resolution ICP with heuristic closest point search for fast and robust 3D registration of range images. In Proceedings of the Fourth International Conference on 3-D Digital Imaging and Modeling, Ban, AB, Canada, 6–13 October 2003; pp. 427–433. 42. Eggert, D.; Dalyot, S. Octree-based simd strategy for icp registration and alignment of 3d point clouds. ISPRS Ann. Photogramm. Remote Sens. Spat. Inf. Sci. 2012, I–3, 105–110. [CrossRef] 43. Huber, P.J. Robust Estimation of a Location Parameter. In Breakthroughs in Statistics: Methodology and Distribution; Kotz, S., Johnson, N.L., Eds.; Springer New York: New York, NY, USA, 1992; pp. 492–518. ISBN 978-1-4612-4380-9. 44. Zhao, Q.; Han, X.H.; Chen, Y.W. A robust registration method using Huber ICP and low rank and sparse decomposition. In Proceedings of the 2015 Asia-Paciﬁc Signal and Information Processing Association Annual Summit and Conference (APSIPA), Hong Kong, China, 16–19 December 2015; pp. 744–752. 45. Zhllin, L. Robust surface matching for automated detection of local deformations using least-median- of-squares estimator. Photogramm. Eng. Remote Sens. 2001, 67, 1283–1292. 46. Masuda, T.; Sakaue, K.; Yokoya, N. Registration and integration of multiple range images for 3-D model construction. In Proceedings of the 13th International Conference on Pattern Recognition, Vienna, Austria, 25–29 August 1996; pp. 879–883. 47. Bergström, P.; Edlund, O. Robust registration of point sets using iteratively reweighted least squares. Comput. Optim. Appl. 2014, 58, 543–561. [CrossRef] 48. Leica Geosystems. Leica GS10/GS15 User Manual. Available online: http://www.surveyequipment.com/ PDFs/Leica_Viva_GS10_GS15_User_Manual.pdf (accessed on 1 February 2019). 49. IEC. Maritime Navigation and Radiocommunication Equipment and Systems—Digital Interfaces—Part 3: Multiple Talker and Multiple Listeners. High Speed Network Bus; IEC: Geneve, Switzerland, 2014. 50. NMEA. National Marine Electronics Association. Available online: https://www.nmea.org/content/nmea_ standards/v411.asp (accessed on 5 February 2019). 51. Data Repository. Available online: https://github.com/lukisp2/ICP_DATA_MARINA (accessed on 16 August 2019). © 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/). http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png Applied Sciences Multidisciplinary Digital Publishing Institute http://www.deepdyve.com/lp/multidisciplinary-digital-publishing-institute/use-of-a-weighted-icp-algorithm-to-precisely-determine-usv-movement-7W4cq0HlEI

Loading next page...

References (55)

Hao Men, Biruk Gebre, K. Pochiraju (2011)
Color point cloud registration with 4D ICP algorithm
2011 IEEE International Conference on Robotics and Automation
F. Forst (1980)
On robust estimation of the location parameter
Alexander Marinov, N. Zlateva, D. Dimov, Delian Marinov (2012)
Weighted ICP Algorithm for Alignment of Stars from Scanned Astronomical Photographic Plates
Serdica Journal of Computing
Yuze Wang, Xin Chen, Peilin Liu (2018)
Statistical Multipath Model Based on Experimental GNSS Data in Static Urban Canyon Environment
Sensors (Basel, Switzerland), 18
(2012)
Improving the Performance of ICP for Real-Time Applications using an Approximate Nearest Neighbour Search
P. Bergström, O. Edlund (2014)
Robust registration of point sets using iteratively reweighted least squares
Computational Optimization and Applications, 58
(2011)
Scansce . User ’ s Manual and Technical Specifications
C. Allen (2012)
The Seabots are Coming Here: Should They Be Treated as 'Vessels'?
Journal of Navigation, 65
Ying He, Bin Liang, Jun Yang, Shunzhi Li, Jin He (2017)
An Iterative Closest Points Algorithm for Registration of 3D Laser Scanner Point Clouds with Geometric Features
Sensors (Basel, Switzerland), 17
E. Mendes, P. Koch, S. Lacroix (2016)
ICP-based pose-graph SLAM
2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR)
T. Masuda, K. Sakaue, N. Yokoya (1996)
Registration and integration of multiple range images for 3-D model construction
Proceedings of 13th International Conference on Pattern Recognition, 1
User's Manual and Technical Specifications
P. Xie, M. Petovello (2015)
Measuring GNSS Multipath Distributions in Urban Canyon Environments
IEEE Transactions on Instrumentation and Measurement, 64
(2000)
Performance Standards for Marine Transmitting Heading Devices (thds)
F. Pomerleau, F. Colas, R. Siegwart, Stéphane Magnenat (2013)
Comparing ICP variants on real-world data sets
Autonomous Robots, 34
Riccardo Polvara, Sanjay Sharma, Jian Wan, Andrew Manning, R. Sutton (2017)
Obstacle Avoidance Approaches for Autonomous Navigation of Unmanned Surface Vehicles
Journal of Navigation, 71
Edwin Olson (2008)
Robust and efficient robotic mapping
G. Godin, M. Rioux, R. Baribeau (1994)
Three-dimensional registration using range and intensity information
, 2350
(2000)
Adoption of Amendments to Performance Standards for Devices to Measure and Indicate Speed and Distance
Zhengyou Zhang (1994)
Iterative point matching for registration of free-form curves and surfaces
International Journal of Computer Vision, 13
Yue Wang, R. Xiong, Qianshan Li (2013)
Em-Based Point to Plane ICP for 3D Simultaneous Localization and Mapping
Int. J. Robotics Autom., 28
K. Naus, M. Wąż (2015)
Precision in Determining Ship Position using the Method of Comparing an Omnidirectional Map to a Visual Shoreline Image
Journal of Navigation, 69
D. Eggert, S. Dalyot (2012)
OCTREE-BASED SIMD STRATEGY FOR ICP REGISTRATION AND ALIGNMENT OF 3D POINT CLOUDS
ISPRS Annals of the Photogrammetry, Remote Sensing and Spatial Information Sciences
M. Moratuwage, W. Wijesoma, B. Kalyan, J. Dong, P.G.C. Senarathne, F. Hover, N. Patrikalakis (2010)
Collaborative multi-vehicle localization and mapping in marine environments
OCEANS'10 IEEE SYDNEY
Zhilin Li, Zhu Xu, M. Cen, X. Ding (2001)
Robust surface matching for automated detection of local deformations using least-median-of-squares estimator
Photogrammetric Engineering and Remote Sensing, 67
S. Rusinkiewicz, M. Levoy (2001)
Efficient variants of the ICP algorithm
Proceedings Third International Conference on 3-D Digital Imaging and Modeling
Rong Wang, Z. Geng (2015)
WA-ICP algorithm for tackling ambiguous correspondence
2015 3rd IAPR Asian Conference on Pattern Recognition (ACPR)
H. Durrant-Whyte, Tim Bailey (2006)
Simultaneous localization and mapping: part I
IEEE Robotics & Automation Magazine, 13
E. Shafiee, M. Mosavi, M. Moazedi (2017)
Detection of Spoofing Attack using Machine Learning based on Multi-Layer Neural Network in Single-Frequency GPS Receivers
Journal of Navigation, 71
M. Greenspan, Mike Yurick (2003)
Approximate k-d tree search for efficient ICP
Fourth International Conference on 3-D Digital Imaging and Modeling, 2003. 3DIM 2003. Proceedings.
(2009)
Ripping Radar Image with the Sea Map Using the Alignment Method
(2019)
Leica GS10/GS15 User Manual. Available online: http://www.surveyequipment.com/ PDFs/Leica_Viva_GS10_GS15_User_Manual.pdf (accessed on 1 February 2019)
Abdenour Amamra, N. Aouf, Dowling Stuart, M. Richardson (2015)
A recursive robust filtering approach for 3D registration
Signal, Image and Video Processing, 10
Rui Guo, Fengchi Sun, Jing Yuan (2009)
ICP based on Polar Point Matching with application to Graph-SLAM
2009 International Conference on Mechatronics and Automation
Jonas Callmer, David Törnqvist, F. Gustafsson, Henrik Svensson, Pelle Carlbom (2011)
Radar SLAM using visual features
EURASIP Journal on Advances in Signal Processing, 2011
Juan Nieto, T. Bailey, E. Nebot (2007)
Recursive scan-matching SLAM
Robotics Auton. Syst., 55
T. Jost, Heinz Hugli (2003)
A multi-resolution ICP with heuristic closest point search for fast and robust 3D registration of range images
Fourth International Conference on 3-D Digital Imaging and Modeling, 2003. 3DIM 2003. Proceedings.
A. Nowak (2015)
The Proposal to “Snapshot” Raim Method for Gnss Vessel Receivers Working in Poor Space Segment Geometry
Polish Maritime Research, 22
Jaromír Konecny, Michal Prauzek, P. Krömer, P. Musílek (2016)
Novel Point-to-Point Scan Matching Algorithm Based on Cross-Correlation
Mob. Inf. Syst., 2016
(2014)
and Radiocommunication Equipment and Systems—Digital Interfaces—Part 3: Multiple Talker and Multiple Listeners
A. Censi (2008)
An ICP variant using a point-to-line metric
2008 IEEE International Conference on Robotics and Automation
(2010)
Ripping Radar Image with the Sea Map Using the Alignment Method 2009–2019; Logistyka, Institute of Logistics and Warehousing
This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license
Leica GS10/GS15 User Manual
K. Naus, M. Wąż (2011)
Accuracy in fixing ship’s positions by camera survey of bearings
Geodesy and Cartography, 60
D. Ribas, P. Ridao, José Neira (2010)
Underwater SLAM for Structured Environments Using an Imaging Sonar
, 65
Jiaolong Yang, Hongdong Li, Yunde Jia (2013)
Go-ICP: Solving 3D Registration Efficiently and Globally Optimally
2013 IEEE International Conference on Computer Vision
National Marine Electronics Association
Qiaochu Zhao, X. Han, Yenwei Chen (2015)
A robust registration method using Huber ICP and low rank and sparse decomposition
2015 Asia-Pacific Signal and Information Processing Association Annual Summit and Conference (APSIPA)
Wendy Navarro, J. Velez, A. Orfila (2018)
Estimation of sea state parameters using X-band marine radar technology in coastal areas
, 10773
F. Amigoni, Simone Gasparini, Maria Gini (2004)
Scan Matching Without Odometry Information
(2014)
Maritime Navigation and Radiocommunication Equipment and Systems-Digital Interfaces-Part 3: Multiple Talker and Multiple Listeners. High Speed Network Bus
J. Bentley (1975)
Multidimensional binary search trees used for associative searching
Commun. ACM, 18
Aleksandr Segal, D. Hähnel, S. Thrun (2009)
Generalized-ICP
P. Besl, N. McKay (1992)
A Method for Registration of 3-D Shapes
IEEE Trans. Pattern Anal. Mach. Intell., 14

Publisher: Multidisciplinary Digital Publishing Institute
Copyright: © 1996-2019 MDPI (Basel, Switzerland) unless otherwise stated Terms and Conditions Privacy Policy
ISSN: 2076-3417
DOI: 10.3390/app9173530
Publisher site: See Article on Publisher Site

Abstract

applied sciences Article Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters Krzysztof Naus * and Łukasz Marchel * Institute of Navigation and Maritime Hydrography, Polish Naval Academy, 81-103 Gdynia, Poland * Correspondence: k.naus@amw.gdynia.pl (K.N.); l.marchel@amw.gdynia.pl (Ł.M.) Received: 1 August 2019; Accepted: 23 August 2019; Published: 28 August 2019 Abstract: The purpose of this article is to present a study aimed at developing a method for the precise determination of unmanned surface vehicle (USV) movement parameters (heading (HDG), speed over ground (SOG) and rate of turn (ROT)) through appropriate processing. The technique employs a modiﬁed weighted ICP (Iterative Closest Point) algorithm and a 2D points layer arranged in the horizon plane obtained from measurements. This is performed with the help of Light Detection and Ranging (LIDAR). A new method of weighting is presented. It is based on a mean error in a given direction and the results of modiﬁed weighted ICP tests carried out on the basis of ﬁeld measurement data. The ﬁrst part of the paper characterizes LIDAR measuring errors and indicates the possibilities for their use in matching point clouds. The second part of the article deals with a method for determining the SOG and course over ground (COG), based on a modiﬁed weighted ICP algorithm. The main part of the paper reviews a test method aimed at evaluating the accuracy of determining the SOG and COG by the scan-matching method using a modiﬁed weighted ICP algorithm. The ﬁnal part presents an analysis comparing the obtained SOG and COG results with reference results of GNSS RTK measurements and the resulting generalised conclusions. Keywords: ICP; mean error ellipse; precise maneuvering; movement parameters 1. Introduction Heading (HDG), speed over ground (SOG) and rate of turn (ROT) are basic movement parameters which characterize the way a ship’s hull moves in relation to the Earth’s surface. They are crucial in the decision-making process, particularly when navigating in congested areas, including harbors and rivers. Currently, these parameters are most frequently determined on a ship using a gyrocompass and a log, i.e., navigational instruments whose measuring accuracy, according to the International Maritime Organization (IMO) requirements, is not required to be very high: “Errors in the indicated speed, when the ship is operating free from shallow water eect and from the eects of wind, current and tide, should not exceed 2% of the speed of the ship, or 0.2 knots, whichever is greater” [1], “The follow-up error for dierent rates of turn should be: less than 0.5 at rates up to 10 /s; and less than 1.5 between a rate of 10 /s and 20 /s” [2], although it determines the precision and the ability to control the ship’s movement which, consequently, may aect the navigational safety level. It is therefore reasonable to carry out scientiﬁc research aimed at searching for new measuring and data processing methods in order to minimize measuring errors in a ship’s movement parameters. For many years, maritime navigation has placed increasing emphasis on the development of methods to determine position and navigational parameters that could oer an alternative to the GNSS system [3]. In the age of the development of unmanned surface vehicles (USVs) whose main tasks increasingly involve independent (autonomous) maneuvering in restricted areas, it is necessary to develop new methods to obtain precise (accurate) navigational movement parameters (in particular the HDG, SOG, course over ground (COG), and ROT) [4,5]. Basing the USV decision-making process Appl. Sci. 2019, 9, 3530; doi:10.3390/app9173530 www.mdpi.com/journal/applsci Appl. Sci. 2019, 9, 3530 2 of 17 exclusively on the parameters of movement and positions obtained from satellite systems is unjustiﬁed, particularly in port basins in which infrastructure facilities and other ships may suppress and reﬂect the system signals [6–8]. Another reason why one should not rely exclusively on such solutions is the new techniques of satellite signal jamming and spooﬁng [9]. A good way to avoid the satellite system inconveniences may be the development of locally operating systems whose actuator is located on a carrier ship, making their operation more dicult to interfere with. A system with a local range of operation needs no special, external infrastructure to generate correct data. Recently, the clear development of devices such as Light Detection and Ranging (LIDAR) [10] has been noted, with increased capacities in terms of the range, accuracy of generated data and the purchase price. Certain devices have a range above 500 m and high accuracy in angle and distance measurements, which enables more widespread use of such devices in sea-going ship navigation. It should also be noted that the measuring capacity of such sensors is not limited by the radar ’s dead zone at close ranges. On the other hand, such a phenomenon is typical of pulse radars which are commonly used on almost every vessel [11]. This article presents a method for the precise determination of the SOG and COG parameters through processing, using a modiﬁed iterative closest point (ICP) algorithm [12–15] of a horizontal 2D layer of cloud points obtained from LIDAR measurements. The main process is based on the simultaneous localization and mapping (SLAM) method. In simple terms, this is a method allowing a map of an unknown environment to be built, which, after processing, is used for navigating in a particular area. This technique was isolated as a separated area of interest in 1996. The SLAM evolution was described in [16]. SLAM is rarely used in determining the position of surface vessels. Only a few applications of the method in the context of its use on an open sea surface are known [17]. On the other hand, SLAM has become very popular in determining the position of autonomous underwater vehicles (AUV). A review of the methods applied in determining the positions of AUVs can be found in [18]. Since SLAM is already a relatively well-studied method, many approaches to solving a particular position-determining problem have emerged. One of the most common solutions to the problem is the so-called scan-matching. This set of techniques involves matching the subsequent scans of the surrounding environment which are obtained while exploring a particular area [19]. There are numerous algorithms variants used to match scans obtained by mapping devices. These include the ICP algorithm [20,21], ICL [22] and algorithms based on matching histograms of the distance function [23]. However, given the dierent methods of matching point clouds, the ICP algorithm proposed for the ﬁrst time in [24] outperforms other algorithms in its simplicity and the possibility for its continuous development. Considering the use of the SLAM technique to determine the parameters of the vessel’s movement, it is worth looking at the 2011 article by Callmer et al. [25]. In this case, the authors use the object approach (landmark) based on the SIFT algorithm and the extended Kalman ﬁlter (EKF), and images from the navigation radar. The results of the work are promising; however, in this case there is a signiﬁcant drift of coordinates of the position and parameters of the vessels movement. Another publication describing the implementation of the SLAM task at sea [17]. The approach presented here assumes that the map of the environment will be explored on a simultaneous basis by more than one vehicle/ship. This assumption allows obtaining a much more accurate model of the map of the environment (global map) by combining many sub-maps obtained by vehicles. This mapping is used to position each of them. The approach presented in the article is a scan matching without odometry information. A similar solution, but based on another scan matching algorithm, was presented in [26]. It involves adjusting concurrent scans one after another without using dead reckoning data in the alignment algorithm. This article uses the ICP algorithm with the error metric in the point-to-point version [27]. Other versions of the metrics are: point-to-line [28], point-to-plane [29], and plane-to-plane [30]. The point-to-point metric was chosen because of the intuitive way of attaching importance to individual correspondences Appl. Sci. 2019, 9, 3530 3 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 3 of 17 (based solely on the coordinate error of the position of a pair of points), without the need to consider need to consider the uncertainty of the position of the line or plane (for cases of three-dimensional the uncertainty of the position of the line or plane (for cases of three-dimensional point clouds). point clouds). In this article, the emphasis is put on the methods of weighting on the basis of estimated errors In this article, the emphasis is put on the methods of weighting on the basis of estimated errors of the position coordinates of a pair of corresponding points. The problem of weighting of pairs in of the position coordinates of a pair of corresponding points. The problem of weighting of pairs in a a navigational approach is presented in [31], and for other applications in [32,33]. This publication navigational approach is presented in [31], and for other applications in [32, 33]. This publication assumes that pairs with a higher distance value between points will be given lower weights compared assumes that pairs with a higher distance value between points will be given lower weights compared to those at a smaller distance from one another. Weighting based on color was also accepted. Another to those at a smaller distance from one another. Weighting based on color was also accepted. Another approach presented in [21] is the weighting depending on the uncertainty of the position of points in approach presented in [21] is the weighting depending on the uncertainty of the position of points in the scan and the uncertainty of the position of the straight line (in the point-to-line variant). It was the scan and the uncertainty of the position of the straight line (in the point-to-line variant). It was assumed that more weight will be put on pairs that have less inﬂuence on the error metric. A similar assumed that more weight will be put on pairs that have less influence on the error metric. A similar approach is presented in [34]. The authors carry out tests on three variants of the error measure: point, approach is presented in [34]. The authors carry out tests on three variants of the error measure: point, line and plane. The approach proposed in this article is similar to this; however, it assumes a dierent, line and plane. The approach proposed in this article is similar to this; however, it assumes a different, more universal distribution of coordinates error. They can be easily implemented not only in the case more universal distribution of coordinates error. They can be easily implemented not only in the case of a lidar sensor, but also in the case of navigation radar. of a lidar sensor, but also in the case of navigation radar. 2. Evaluation of the Accuracy of Determining the Coordinates of a Position Using LIDAR 2. Evaluation of the Accuracy of Determining the Coordinates of a Position Using LIDAR The determination of horizontal coordinates using LIDAR is carried out based on the angle The determination of horizontal coordinates using LIDAR is carried out based on the angle 𝛼 and the distance r measured between the sensor and the target, i.e., the light signal reﬂection point and the distance 𝑟 measured between the sensor and the target, i.e., the light signal reflection point (Figure 1). (Figure 1). Fig Figure ure 1. 1. The The prin principle ciple of m of eameasuring suring 2D h2D oriz horizontal ontal coordi coor natdinates es (x, y)( x usi , y n)gusing Light Light DetectDetection ion and Ra and nging Ranging (LIDAR). (LIDAR) The mean error M and the parameters of mean error ellipse (i.e., the lengths of semi-axes 𝑎 and The mean error M and the parameters of mean error ellipse (i.e., the lengths of semi-axes a and b) 𝑏 ) of the determination of 2D horizontal coordinates (x, y) using LIDAR can be calculated by ( ) of the determination of 2D horizontal coordinates x, y using LIDAR can be calculated by applying applying the law of mean error propagation [35, 36]. When the functions of a single result of the the law of mean error propagation [35,36]. When the functions of a single result of the following following measurement are known: measurement are known: = arc tg , (1) 𝛼 = arc tg , (1) 2 2 (2) 𝑟 = √𝑥 + 𝑦 , Appl. Sci. 2019, 9, 3530 4 of 17 2 2 r = x + y , (2) equations of the mean errors of positional lines can be written as: ! ! 2 2 @ @ = / + , (3) l() @x @y ! ! 2 2 @r @r = / + , (4) l(r) @x @y which enable the determination of mean error of position: 2 2 2 M = + = ( r) + , (5) l() l(r) sin a = r, (6) b = , (7) where: —the angle of intersection of positional lines, which for LIDAR measurements is always equal to 90 , —mean error of the angle measurement , —mean error of the distance measurement r, r—distance between the sensor and the target, a—the length of the long semi-axis of mean error ellipse (see Figure 2), b—the length of the short semi-axis of mean error ellipse (see Figure 2). It follows from Dependence (5) that the value of mean error M and the length of the long semi-axis a of the mean error ellipse are signiﬁcantly aected by the distance r between the sensor and the target. The calculations assumed that the measurements were performed using the Scanse Sweep LIDAR [37] for which the manufacturer provides = 0.004258r (rounded value). Thereby one takes into account the fact that the error value of the angle measurement does not only depend on the beam’s divergence (footprint of the beam increases as a function of divergence) and the angle of its incidence on the surface of the object, but also on the accuracy of measuring the angle change by the optical head encoder that can be aected by environmental conditions such as temperature and humidity, vibrations and imperfections in the performance of mechanical elements that cooperate with each other. This can be characteristic for entry level LIDAR devices. The spatial angle resolution can be derived from sampling frequency that is set up to 1000 Hz (when optical sensor is spinning with frequency 1 Hz the Lidar is able to sample up to 1000 point for a single spin, 5 Hz—200 points, 10 Hz—100 points). The range spatial resolution is around 0.1 cm. Based on the known lengths of semi-axes a and b of the mean error ellipse, and based on the known direction of measurement, one can simply generate a covariance matrix cov of the (x, y) vector coordinates: 2 3 2 2 2 2 2 6 a cos + b sin a b sin cos 7 a a a a 6 7 6 7 cov(x, y) = 6 7, (8) 4 5 2 2 2 2 2 2 a b sin cos a sin + b cos a a a a where —is the angle between the semi-axis a of the mean error ellipse and the axis OY (for LIDAR measurements, equal to + 90 ). Then, using this, to determine the mean square error of the determination of the position coordinates, but in the speciﬁed direction : Appl. Sci. 2019, 9, 3530 5 of 17 2 T 2 2 2 2 = S cov(x, y)S = sin sin r sin + cos a r a 2 2 2 cos cos sin r + a a r (9) 2 2 2 2 + sin sin r cos + sin a r a 2 2 2 cos sin sin r , a a r h i where S = cos sin . Appl. Sci. 2019, 9, x FOR PEER REVIEW 5 of 17 Figure 2. Mean error of the determination of position coordinates in the specified direction. Figure 2. Mean error of the determination of position coordinates in the speciﬁed direction. (calculated (calculated using the covariance matrix). using the covariance matrix). 2.1. The Determination of USV Movement Parameters Using the Scan-Matching Method 2.1. The Determination of USV Movement Parameters Using the Scan-Matching Method The taxonomy of the ICP algorithm is carried out in the following successive stages of data The taxonomy of the ICP algorithm is carried out in the following successive stages of data processing [21]: processing [21]: Selection (selection of points which are suitable for the alignment); • Selection (selection of points which are suitable for the alignment); Matching (matching points from model cloud to reference points cloud); • Matching (matching points from model cloud to reference points cloud); • Weighting (giving weight to corresponding points); Weighting (giving weight to corresponding points); • Rejecting (deleting some points based on the robust criterion function); Rejecting (deleting some points based on the robust criterion function); • Assigning an error metric (choosing point-to-point, point-to-line, point-to-plane or plane-to- Assigning an error metric (choosing point-to-point, point-to-line, point-to-plane or plane-to-plane plane error metric); error metric); • Minimizing the metric between selected points. Minimizing the metric between selected points. Selection. Let us assume that a USV determines the change in its position through the translation Selection. Let us assume that a USV determines the change in its position through the translation vector 𝐓 (𝑘 + 1) and the rotation matrix 𝐑 (𝑘 + 1) determined based on two maps of the ( ) ( ) vector T k + 1 and the rotation matrix R k + 1 determined based on two maps of the surrounding surrounding environment 𝐌 (𝑘 ) and 𝐌 (𝑘 + 1), constructed at the moment (𝑘 ) and the moment environment M(k) and M(k + 1), constructed at the moment (k) and the moment (k + 1) following it ( ) ( ) 𝑘 + 1 following it (e.g., 2D horizontal layer of cloud points for LIDAR measurements). Let ℤ 𝑘 ( ) ( ) (e.g., 2D horizontal layer of cloud points for LIDAR measurements). Let Z k and Z k + 1 represent and ℤ(𝑘 + 1) represent two sets {(𝑟 (𝑘 ), (𝑘 )), … , (𝑟 (𝑘 ), (𝑘 ))} and {(𝑟 (𝑘 + 1), (𝑘 + 1 1 𝑛 𝑛 1 1 two sets (r (k), (k)), ::: , (r (k), (k)) and (r (k + 1), (k + 1)), ::: , (r (k + 1), (k + 1)) of 1 1 n n 1 1 n n ) ( ) ( ) 1 ), … , (𝑟 𝑘 + 1 , 𝑘 + 1 )} of measurements of the distance 𝑟 and direction  , carried out in 𝑛 𝑛 measurements of the distance r and direction , carried out in relation to n ﬁeld obstacles (see Figure 1). n o n o relation to 𝑛 field obstacles (see Figure 1). On their basis, set of points 𝐌 (𝑘 ) = {𝐩 (𝑘 ), … , 𝐩 (𝑘 )} and 1 n On their basis, set of points M(k) = p (k), ::: , p (k) and M(k + 1) = p (k + 1), ::: , p (k + 1) 1 n 1 n { } 𝐌 (𝑘 + 1) = 𝐩 (𝑘 + 1), … , 𝐩 (𝑘 + 1) will be determined, where the coordinates of each point in the 1 n will be determined, where the coordinates of each point in the formula are calculated via the formula are calculated via the following dependence: following dependence: " # 𝐩 = [ ], p = , i 𝑦 (10) x = r cos( ), i i i (10) 𝑥 = 𝑟 ∙ cos ( ), 𝑖 𝑖 𝑖 y = r sin( ), i i i 𝑦 = 𝑟 ∙ sin( ), 𝑖 𝑖 𝑖 where: i = 1,::: , n where: ( ) ( ) They will be used to determine the translation vector T k + 1 and the rotation matrix R k + 1 𝑖 = 1, … , 𝑛 using the singular value decomposition (SVD) method [38], which will be carried out in combination They will be used to determine the translation vector 𝐓 (𝑘 + 1) and the rotation matrix 𝐑 (𝑘 + 1) using the singular value decomposition (SVD) method [38], which will be carried out in combination with a change in the position (in subsequent iteration steps) of the set of points from 𝐌 (𝑘 ) to 𝐌 (𝑘 + 1) using the modified weighted ICP method. In reality, there is very little probability that both sets of points 𝐌 (𝑘 ) and 𝐌 (𝑘 + 1) will contain the same number of points. Equalising the number of points is required for the family of ICP algorithms with point-to-point error metric and therefore, in the calculations they were carried out by reducing the number of points. From a more numerous set, the points were randomly removed so that their number would be equal to the number of the points from a smaller set. Appl. Sci. 2019, 9, 3530 6 of 17 with a change in the position (in subsequent iteration steps) of the set of points from M(k) to M(k + 1) using the modiﬁed weighted ICP method. In reality, there is very little probability that both sets of points M(k) and M(k + 1) will contain the same number of points. Equalising the number of points is required for the family of ICP algorithms with point-to-point error metric and therefore, in the calculations they were carried out by reducing the number of points. From a more numerous set, the points were randomly removed so that their number would be equal to the number of the points from Appl. Sci. 2019, 9, x FOR PEER REVIEW 6 of 17 a smaller set. Matching. Literature shows that there are many ways to match points in pairs. One of them Matching. Literature shows that there are many ways to match points in pairs. One of them is is the k-d tree method presented in [39,40]. Also considered are methods based on the heuristic the k-d tree method presented in [39,40]. Also considered are methods based on the heuristic search search of pairs to reduce the complexity of calculations [41]. In this article, correspondence between of pairs to reduce the complexity of calculations [41]. In this article, correspondence between points points is created based on the method presented in [42]. It is based on the Delaunay triangulation. is created based on the method presented in [42]. It is based on the Delaunay triangulation. The The method generates a two-dimensional grid of triangles based on the reference set M(k). Next, using ( ) method generates a two-dimensional grid of triangles based on the reference set 𝐌 𝑘 . Next, using ( ) the nearest neighbor method, the nearest apex of triangles to the points from the set M k + 1 are found the nearest neighbor method, the nearest apex of triangles to the points from the set 𝐌 (𝑘 + 1) are (points from M(k)) in such a way that for each point p (k) the nearest point p (k + 1) is assigned i id(i) ( ) ( ) ( found (points from 𝐌 𝑘 ) in such a way that for each point 𝐩 𝑘 the nearest point 𝐩 𝑘 + 𝒊 𝒊𝒅 (𝒊 ) (see Figure 3). 1 is assigned (see Figure 3). Figure 3. Left side—2-point clouds before Dalaunay triangulation; right side—Dalaunay triangulation Figure 3. Left side—2-point clouds before Dalaunay triangulation; right side—Dalaunay triangulation and nearest neighbor match (dashed lines). and nearest neighbor match (dashed lines). Weighting of the pairs. The modiﬁcation of the ICP method will involve the application of the Weighting of the pairs. The modification of the ICP method will involve the application of the weighting factor w (k, k + 1) for calculations for each pair of points (nearest neighbors): 1,:::,n weighting factor 𝑤 (𝑘 , 𝑘 + 1) for calculations for each pair of points (nearest neighbors): 1,…,𝑛 1 1 𝑤 (𝑘 , 𝑘 + 1) = , w (k, k + 1) = p , ( (11) 11) √𝐺 (𝑘 ,𝑘 +1) G (k, k + 1) ( ) ( ) where 𝐺 𝑘 , 𝑘 + 1 is the sum of mean errors of the determination of the coordinates of points 𝐩 𝑘 𝑖 𝑖 where G (k, k + 1) is the sum of mean errors of the determination of the coordinates of points p (k) and 𝐩 (𝑘 + 1) in the specified direction 𝛽 (see Figure 2): (𝑖 ) and p (k + 1) in the speciﬁed direction (see Figure 2): id(i) 2 2 𝐺 (𝑘 , 𝑘 + 1) = 𝜎 (𝑘 ) + 𝜎 (𝑘 + 1), 𝑖 𝛽 𝛽 (12) 𝑖 (𝑖 ) 2 2 G (k, k + 1) = (k) + (k + 1), (12) id(i) where 𝜎 (𝑘 ) is the mean error of the determination of the coordinates of the point 𝐩 (𝑘 ) in the 𝛽 𝑖 direction of 𝛽 (from 𝐩 (𝑘 ) to 𝐩 (𝑘 + 1)), 𝜎 (𝑘 + 1) is the mean error of the determination of 𝑖 (𝑖 ) 𝛽 where (k) is the mean error of the determination of the coordinates of the point p (k) in the direction (𝑖 ) of th e (fr co om ord pin(a ktes ) to o pf th(e k + poi 1) n)t, 𝐩 (𝑘 + (k1+ ) 1i) n isth the e d mean irectio err nor of of 𝛽 the (fdetermination rom 𝐩 (𝑘 + of 1) the tocoor 𝐩 ( dinates 𝑘 )) , or 𝑖 (𝑖 ) 𝑖 i id(i) id(i) of 𝐺 the (𝑘 , poi 𝑘 +nt 1) p i(s k + the 1)m in ea the n er dir ro ection r of th ofe d (fr ete om rmi pnati(o kn+ o1 f) th toe pcoo (k)rd ), or ina G tes (k,ok f + th1 e ) po is the intsmean 𝐩 (𝑘 ) err aor nd 𝑖 i 𝑖 i id(i) i of the ( determination ) of the coordinates of the points p (k) and p (k + 1) (see Figure 1): 𝐩 𝑘 + 1 (see Figure 1): (𝑖 ) i id(i) 2 2 2 2 ( ) ( ) ( ) G 𝐺 (k𝑘, k , 𝑘 ++ 1) 1== M 𝑀 (k) 𝑘 ++ M 𝑀 (k 𝑘++ 1) 1 , , ( (13) 13) i 𝑖 𝑖 (𝑖 ) id(i) where 𝑀 (𝑘 ) is the mean error of the determination of the coordinates of the point 𝐩 (𝑘 ) , 𝑖 𝑖 where M (k) is the mean error of the determination of the coordinates of the point p (k), M (k + 1) i id(i) ( ) ( ) 𝑀 𝑘 + 1 is the mean error of the determination of the coordinates of the point 𝐩 𝑘 + 1 . (𝑖 ) (𝑖 ) ( ) is the mean error of the determination of the coordinates of the point p k + 1 . id(i) Weighting step gives specific calculated value for each pair. Then in further processing heavier pairs involve more the translation and rotation parameters (Figure 4). 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 Appl. Sci. 2019, 9, 3530 7 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 7 of 17 Weighting step gives speciﬁc calculated value for each pair. Then in further processing heavier Appl. Sci. 2019, 9, x FOR PEER REVIEW 7 of 17 pairs involve more the translation and rotation parameters (Figure 4). Figure 4. Weighting due to proposed error factor. Bolted, dashed line equals more weight for specific Figure 4. Weighting due to proposed error factor. Bolted, dashed line equals more weight for pair. Figure 4. Weighting due to proposed error factor. Bolted, dashed line equals more weight for specific speciﬁc pair. pair. Rejection. Due to the relatively high noise level of many scans, particularly in the sectors Rejection. Due to the relatively high noise level of many scans, particularly in the sectors illustrating illustrating long distance measurements, the Huber function [43] which attenuated the outlying Rejection. Due to the relatively high noise level of many scans, particularly in the sectors long distance measurements, the Huber function [43] which attenuated the outlying measurement measurement results (affected by gross errors) was also applied in the calculations. This function was illustrating long distance measurements, the Huber function [43] which attenuated the outlying results (aected by gross errors) was also applied in the calculations. This function was arbitrarily arbitrarily selected from among many functions known and commonly used for this purpose [21, 31, measurement results (affected by gross errors) was also applied in the calculations. This function was selected from among many functions known and commonly used for this purpose [21,31,44–47]. 44 a– rb 47 itra ]. ri The ly sel roec bust ted f fun rom cti a om n ore ng jec m ts a n oy r fs un ign cti ifo ica ns nkn tly ow ren d uce ands co wei mm gh ots nl y oused f the fspe or th ciis fi c pur papo irsse d[ ue 21, t3 o1 , The robust function rejects or signiﬁcantly reduces weights of the speciﬁc pairs due to robustness robustness criterion (see Figure 5). The robust function is based on 𝑘𝐻𝑢 factor presented in 44–47]. The robust function rejects or significantly reduces weights of the specific pairs due to criterion (see Figure 5). The robust function is based on kHu factor presented in Algorithm 1. Rejecting Aro lgo bust rith nm ess 1. cr Rej ite ec rito in ng ( o see r wei Figh gure t m i5 n)i.m The izatio ro nb iust s pe rf fuo n rmed ction bia s sed ba sed on no on rm 𝑘𝐻𝑢 of tw fo a cto pair repr d poi esen nted ts. In i n or weight minimization is performed based on norm of two paired points. In this article the fusion of this article the fusion of the robust function and proposed error weighting is used to derivate the final Algorithm 1. Rejecting or weight minimization is performed based on norm of two paired points. In the robust function and proposed error weighting is used to derivate the ﬁnal weight. wei thigh s at. rti cle the fusion of the robust function and proposed error weighting is used to derivate the final weight. Fig Figure ure 5.5. Re Rejecting jecting pa pairs irs due due to to rob robust ust crcriterion. iterion. Figure 5. Rejecting pairs due to robust criterion. Assigning an error metric. The ﬁnal function minimizing the value of the matching error E, used Assigning an error metric. The final function minimizing the value of the matching error E, used to accurately determine the translation vector T(k + 1) and the rotation matrix R(k + 1), will take the to accurately determine the translation vector 𝐓 (𝑘 + 1) and the rotation matrix 𝐑 (𝑘 + 1), will take Assigning an error metric. The final function minimizing the value of the matching error E, used following form: the following form: to accurately determine the translation vector 𝐓 (𝑘 + 1) and the rotation matrix 𝐑 (𝑘 + 1), will take E(R, T) = w (k, k + 1)kRp (k) + T p (k + 1)k . (14) the following form: 2 i id(i) i=1 (14) E(𝐑 , 𝐓 ) = ∑ 𝑤 (𝑘 , 𝑘 + 1) ∙ ‖𝐑 𝐩 (𝑘 ) + 𝐓 − 𝐩 (𝑘 + 1)‖ . 𝑖 =1 𝑖 𝑖 (𝑖 ) Provided that points p (k) and p (k + 1) are located the closest to one another, i.e., are the E(𝐑 , 𝐓 ) = ∑ 𝑤 (𝑘 , 𝑘 + 1) ∙ ‖𝐑 𝐩 (𝑘 ) + 𝐓 − 𝐩 (𝑘 + 1)‖ . (14) i 𝑖 id(i) 𝑖 (𝑖 ) 𝑖 =1 ( ) ( ) Provided that points 𝐩 𝑘 and 𝐩 𝑘 + 1 are located the closest to one another, i.e., are the 𝑖 (𝑖 ) nearest neighbors. This method is based on the point-to-point measure of error. nearest Pro nei vi gh ded bo rs. tha Thi t po s met ints h 𝐩 o( d 𝑘 i )s b an ad sed 𝐩 on th (𝑘 e + po 1i )n t a-re to -lpoi ocan ted t mea the sure closest of er to ro o r. n e another, i.e., are the 𝑖 (𝑖 ) Minimizing the metric between selected points. The method of minimizing the error between Minimizing the metric between selected points. The method of minimizing the error between nearest neighbors. This method is based on the point-to-point measure of error. the corresponding points was carried out on the basis of the SVD (Singular Value Decomposition). the corresponding points was carried out on the basis of the SVD (Singular Value Decomposition). Minimizing the metric between selected points. The method of minimizing the error between The method works equally for the weighted and classical ICP algorithm. In this article a method The method works equally for the weighted and classical ICP algorithm. In this article a method the corresponding points was carried out on the basis of the SVD (Singular Value Decomposition). identical to [47] was used. It is required to determine two weighted average coordinates from the The method works equally for the weighted and classical ICP algorithm. In this article a method cloud m(𝑘 ) and m(𝑘 + 1), dependent on point clouds (𝑘 ), (𝑘 + 1) and assigned weights in the identical to [47] was used. It is required to determine two weighted average coordinates from the previous stage: cloud m(𝑘 ) and m(𝑘 + 1), dependent on point clouds (𝑘 ), (𝑘 + 1) and assigned weights in the previous stage: 𝑖𝑑 𝑖𝑑 𝑖𝑑 𝑖𝑑 Appl. Sci. 2019, 9, 3530 8 of 17 identical to [47] was used. It is required to determine two weighted average coordinates from the cloud m(k) and m(k + 1), dependent on point clouds (k), (k + 1) and assigned weights in the previous stage: Appl. Sci. 2019, 9, x FOR PEER REVIEW 8 of 17 w p (k) i=1 i m(k) = P , (15) 𝑛 i=1 ∑ 𝑤 ∙ p (𝑘 ) 𝑖 =1 𝑖 𝑖 ( ) m 𝑘 = , (15) ∑ 𝑤 w p𝑖 (k + 1) 𝑖 =1 id(i) i=1 id(i) m(k + 1) = P . (16) 𝑛 w i=1 ∑ 𝑤 ∙ p (𝑘 + 1) 𝑖 =1 𝑖𝑑 (𝑖 ) (𝑖 ) m(𝑘 + 1) = . (16) Subsequently, we can move on to determine the∑ weighted covariance matrix, which will be used 𝑖 =1 𝑖 to calculate the innovation of rotation matrix and translation (in a given iteration of the algorithm) Subsequently, we can move on to determine the weighted covariance matrix, which will be used to calculate the innovation of rotation matrix and translation (in a given iteration of the algorithm) w p (k)p (k + 1) i=1 id(i) 𝑛 T C = P m(k)m (k + 1). (17) ∑ (𝑤 ∙ 𝐩 (𝑘 ) ∙ 𝐩 (𝑘 + 1)) 𝑖 =1 𝑖 𝑖 n ( ) id i w T (17) i=1 𝐂 = − m(𝑘 ) ∙ m (𝑘 + 1). 𝑖 =1 𝑖 Using the SVD decomposition method on the C matrix Point cloud k Delaunay triangulation Using the SVD decomposition method on the C matrix Point cloud k+1 Matched pairs C = USV . (18) 𝐂 = 𝐕 . (18) where matrices U, Σ, V are characteristic matrices for the SVD method. By means of decomposition where matrices U, S, V are characteristic matrices for the SVD method. By means of decomposition carried out on designated matrices, we obtain the innovations in given iteration— —of the R carried out on designated matrices, we obtain the innovations in given iteration—it—of the R rotation it rotation matrix and the 𝐓 translation: matrix and the T translation: it R = VU , (19) 𝐑 it = 𝐕 𝐔 , (19) T = m(k) R m(k + 1). (20) it it 4 ( ) ( ) 𝐓 = m 𝑘 − 𝐑 ∙ m 𝑘 + 1 . (20) The obtained values in the R and T matrices are partial values developed in a given iteration iter iter The obtained values in the 𝐑 and 𝐓 matrices are partial values developed in a given 𝑟𝑡𝑖𝑒 𝑟𝑡𝑖𝑒 of the algorithm. Based on this, the set m(k + 1) is translated at last iteration (see Figure 6) and as a iteration of the algorithm. Based on this, the set m(k + 1) is translated at last iteration (see Figure 6) result the translation and rotation is obtained: and as a result the translation and rotation is obtained: R(k + 1) = R :::R , (21) ( ) 𝐑 𝑘 + 1 = 𝐑 1 ∙ … ∙ 𝐑 itn , (21) 1 1 𝑡𝑖𝑛 T(k + 1) = (R T + T ) + ::: + (R T + T ). (22) Point cloud k 1,2,3... Pair’s number 𝐓 (𝑘 + 1) = (𝐑 it+1 ∙ 𝐓 it + 𝐓 it+1 ) + … + (𝐑 itn ∙ 𝐓 itn1 + 𝐓 itn ). (22) +1 𝒊𝒕 +𝟏 itn itn−1 itn Point cloud k Weighted pairs Point cloud k+1 where R and T are initialized values. Point cloud k+1 1,2,3... Pair’s number where 𝐑 it=1 and it 𝐓 =1 are initialized values. =1 =1 1,2,3... Pair’s number Point cloud k Point cloud k+1 Figure 6. Left side—2-point clouds; right side—2-point clouds aligned. Figure 6. Left side—2-point clouds; right side—2-point clouds aligned. " # Dx ∆𝑥 The translation vector T(k + 1) = thus obtained will be used to determine ( ) The translation vector 𝐓 𝑘 + 1 = [ ] thus obtained will be used to determine Dy ∆𝑦 2 2 √∆𝑥 +∆𝑦 2 2 (23) SOG(𝑘 + 1) = , Dx + Dy 𝜟 𝑡 SOG(k + 1) = , (23) Dt ( ) where ∆𝑡 is the distance in time between the moment 𝑘 of performing the measurements of ℤ(𝑘 ), and the moment (𝑘 + 1) of performing subsequent measurements of ℤ(𝑘 + 1). cos𝚫 𝜃 sin𝚫 𝜃 ( ) On the other hand, the final form of the rotation matrix 𝐑 𝑘 + 1 = [ ] will be −sin𝚫 𝜃 cos𝚫 𝜃 used to determine: 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝑖𝑡 𝐔𝚺 𝑖𝑑 Appl. Sci. 2019, 9, 3530 9 of 17 where Dt is the distance in time between the moment (k) of performing the measurements of Z(k), and the moment (k + 1) of performing subsequent measurements of Z(k + 1). " # cosD sinD On the other hand, the ﬁnal form of the rotation matrix R(k + 1) = will be sinD cosD used Apto pl. determine: Sci. 2019, 9, x FOR PEER REVIEW 9 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 9 of 17 arc sin(D) ROT(k + 1) = , (24) Dt arc sin(∆𝜽 ) arc sin(∆𝜽 ) ( ) ROT 𝑘 + 1 = , (24) ROT(𝑘 + 1) = , (24) 𝚫 𝑡 𝚫 𝑡 and and and HDG(k + 1) = HDG + arc sin(D), (25) HDG(𝑘 + 1) = HDG + arc sin(𝚫 𝜃 ), (25) HDG(𝑘 + 1) = HDG 0 + arc sin(𝚫 𝜃 ), (25) where HDG is the initial HDG value determined using other methods (devices) prior to the commencement where HDG of is measur the initement. ial HDG value determined using other methods (devices) prior to the where HDG 0 is the initial HDG value determined using other methods (devices) prior to the commencement of measurement. commencement of measurement. 2.2. Weighted Matching Point Clouds Using the Mean Direction Error 2.2. Weighted Matching Point Clouds Using the Mean Direction Error 2.2. Weighted Matching Point Clouds Using the Mean Direction Error Figure 7 shows that the mean error value changes signiﬁcantly as the function of angle , and the Figure 7 shows that the mean error value changes significantly as the function of angle 𝛽 , and gradient of Figure these 7 changes shows thwil at th l incr e me ease an er while ror vathe lue eccentricity changes signof ifica the ntlellipse y as the isfu appr nctio oaching n of angl 1e (the 𝛽 , aellipse nd the gradient of these changes will increase while the eccentricity of the ellipse is approaching 1 (the the gradient of these changes will increase while the eccentricity of the ellipse is approaching 1 (the will be becoming ﬂat, which occurs where measurements are carried out using LIDAR). Owing to ellipse will be becoming flat, which occurs where measurements are carried out using LIDAR). ellipse will be becoming flat, which occurs where measurements are carried out using LIDAR). these particular properties, this error can usefully determine the value of translation and rotation Owing to these particular properties, this error can usefully determine the value of translation and Owing to these particular properties, this error can usefully determine the value of translation and established while minimalizing the error metric between the points from subsequent measurements rotation established while minimalizing the error metric between the points from subsequent rotation established while minimalizing the error metric between the points from subsequent (e.g., point clouds from LIDAR measurements). Figure 8 presents, in the graphical form, how the measurements (e.g., point clouds from LIDAR measurements). Figure 8 presents, in the graphical measurements (e.g., point clouds from LIDAR measurements). Figure 8 presents, in the graphical positions matched in pairs of points based on the corrections (e.g., those added to the components of form, how the positions matched in pairs of points based on the corrections (e.g., those added to the form, how the positions matched in pairs of points based on the corrections (e.g., those added to the the rotation matrix and translation vector) change; the value of the rotation and translation was made components of the rotation matrix and translation vector) change; the value of the rotation and components of the rotation matrix and translation vector) change; the value of the rotation and depending on the values of the sums of mean errors (Equation (9)) calculated along lines connecting the translation was made depending on the values of the sums of mean errors (Equation (9)) calculated translation was made depending on the values of the sums of mean errors (Equation (9)) calculated ( ) ( ) matched points from k and k + 1 cloud. In order to facilitate the interpretation of the phenomenon, along lines connecting the matched points from (𝑘 ) and (𝑘 + 1) cloud. In order to facilitate the along lines connecting the matched points from (𝑘 ) and (𝑘 + 1) cloud. In order to facilitate the it was inter assumed pretation that of th the e phen mean omerr eno or n, ellipses it was aswer sume ed similar that the inm size. ean error ellipses were similar in size. interpretation of the phenomenon, it was assumed that the mean error ellipses were similar in size. Figure Figure 7. A 7. grap A gra hic phic prpr esentation esentationof of the the corr corrected ected po position sition of of ththe e po points ints ma matche tched in d pa inir pai s urs sinusing g the the Figure 7. A graphic presentation of the corrected position of the points matched in pairs using the sums of mean errors calculated along the lines connecting the matched points. sums of mean errors calculated along the lines connecting the matched points. sums of mean errors calculated along the lines connecting the matched points. Figure 8. A graphic presentation the corrected position using the sums of mean errors CEP (circular Figure 8. A graphic presentation the corrected position using the sums of mean errors CEP (circular Figure 8. A graphic presentation the corrected position using the sums of mean errors CEP (circular error probable) calculated for the matched points. error probable) calculated for the matched points. error probable) calculated for the matched points. As can be seen in Figure 7, greater weight is introduced for the points whose mean errors in the As can be seen in Figure 7, greater weight is introduced for the points whose mean errors in the specified direction (point-to-point direction) is lower–pairs marked as 2 and 3 in Figure 7. The heavier specified direction (point-to-point direction) is lower–pairs marked as 2 and 3 in Figure 7. The heavier Appl. Sci. 2019, 9, 3530 10 of 17 As can be seen in Figure 7, greater weight is introduced for the points whose mean errors in the Appl. Sci. 2019, 9, x FOR PEER REVIEW 10 of 17 speciﬁed direction (point-to-point direction) is lower–pairs marked as 2 and 3 in Figure 7. The heavier weight influences the rotation and translation more and that is why pair 2 and 3 are better aligned weight inﬂuences the rotation and translation more and that is why pair 2 and 3 are better aligned comparing with 1 and 4. The use of such a weighting coefficient allows taking into account the spatial comparing with 1 and 4. The use of such a weighting coecient allows taking into account the spatial distribution of errors according to the selected direction. For comparison, Figure 8 presents, in a distribution of errors according to the selected direction. For comparison, Figure 8 presents, in a graphical form, how the positions matched in pairs of points based on the corrections whose values graphical form, how the positions matched in pairs of points based on the corrections whose values depended on the values of the sums of mean errors (Equation (5)) calculated for the matched points. depended on the values of the sums of mean errors (Equation (5)) calculated for the matched points. As can be seen each pair (1, 2, 3, 4) influences the rotation and translation in similar manner. As can be seen each pair (1, 2, 3, 4) inﬂuences the rotation and translation in similar manner. HDG, SOG, and ROT were determined by processing LIDAR scans archived in data packets in HDG, SOG, and ROT were determined by processing LIDAR scans archived in data packets in accordance with the methodology presented in Section 2.1. The pseudo-code of the program used for accordance with the methodology presented in Section 2.1. The pseudo-code of the program used for the calculations using the authors original weighting factor 𝑤 (𝑘 , 𝑘 + 1) and Huber’s robust function ( ) the calculations using the authors original weighting factor w k, k + 1 and Huber ’s robust function coefficient 𝑘𝐻𝑢 = 𝑐 ∙ Median ‖𝐩 (𝑘 ) − 𝐩 (𝑘 + 1)‖ finally took the following form: coecient kHu = cMediankp (k) p (k + 1)k ﬁnally took the following form: 1,…,𝑛 (1,…𝑛 ) 1,:::,n id(1,:::n) A Algorithm lgorithm 1 1 The Theweighted weighteICP d ICP 𝑰𝒏𝒑𝒖𝒕 : 𝐌 (𝑘 ), 𝐌 (𝑘 + 1), , , 𝐑 = 𝐈 , 𝐓 = 0, 𝑑 = 0, 𝑡 ℎ𝑒𝑟𝑠 ℎ𝑜𝑙𝑑 𝑟 𝛼 𝑠𝑟𝑒 𝑓 𝑜𝑟 𝑖 ← 1 𝑡𝑜 𝑛 ( ) |𝑖𝑛𝑑𝑓 𝑖𝑑 𝑖 = argmin ‖𝐑 𝐩 (𝑘 ) + 𝐓 − 𝐩 (𝑘 + 1)‖ ; 𝑖 𝑖𝑑 (𝑖 ) 𝑖 ∈{1,..,𝑛 } 𝑑𝑒𝑛 ( ) ( ) 𝐻𝑢𝑘 = 𝑐 ∙ Median ‖𝐩 𝑘 − 𝐩 𝑘 + 1 ‖, 𝑐 = 2.0138; 1,…,𝑛 𝑖𝑑 (1,…𝑛 ) 𝑓 𝑜𝑟 𝑖 ← 1 𝑡𝑜 𝑛 − 0.5 2 2 | 𝑤 = (𝜎 (𝑘 ) + 𝜎 (𝑘 + 1)) ; 𝑖 𝛽 𝛽 𝑖 𝑖𝑑 (𝑖 ) 𝑜𝑟 − 0.5 2 2 𝑤 = ((𝑀 (𝑘 ) + 𝑀 (𝑘 + 1))) ; 𝑖 𝑖 𝑖𝑑 (𝑖 ) 𝑑𝑒𝑛 𝑖𝑓 ‖𝐩 (𝑘 ) − 𝐩 (𝑘 + 1)‖ > 𝐻𝑢𝑘 𝑖 𝑖𝑑 (𝑖 ) 𝐻𝑢𝑘 |𝑤 = ∙ 𝑤 ; 𝑖 𝑖 ‖𝐩 (𝑘 ) − 𝐩 (𝑘 + 1)‖ 𝑖 𝑖𝑑 (𝑖 ) [𝐑 , 𝐓 ] ← argmin∑ 𝑤 ‖𝐑 𝐩 (𝑘 ) + 𝐓 − 𝐩 (𝑘 + 1)‖ ; 𝑖 𝑖 𝑖𝑑 (𝑖 ) 𝐑 ,𝐓 𝑖 =1 𝑜𝑟𝑓 𝑖 ← 1 𝑡𝑜 𝑛 |𝐩 (𝒌 ) = 𝐑 ∙ 𝐩 (𝒌 ) + 𝐓 ; 𝑖 𝑖 | 𝑑𝑒𝑛 𝑑 (𝐩 (𝑘 ) − 𝐩 (𝑘 + 1)) 𝑖 id(𝑖 ) 𝑤 = ∑ ; 𝑟𝑠𝑒 𝑖 =1 𝑖𝑓𝑓𝑑 = 𝑤 − 𝑜𝑙 𝑑 ; 𝑟𝑠𝑒 𝑟𝑠𝑒 𝑜𝑙 𝑑 = 𝑤 ; 𝑟𝑠𝑒 𝑟𝑠𝑒 𝑤 ℎ𝑖𝑙𝑒 (𝑖𝑓𝑓𝑑 ≥ 𝑡 ℎ𝑟𝑒𝑠 ℎ𝑜𝑙𝑑 ) 3. The Study and the Analysis of the Obtained Results 3. The Study and the Analysis of the Obtained Results The main aim of the study was to evaluate the accuracy of the determination of the HDG, SOG and ROT using a modiﬁed ICP algorithm, carried out as a result of their comparison with high-accuracy The main aim of the study was to evaluate the accuracy of the determination of the HDG, SOG reference measurements. The study involved: and ROT using a modified ICP algorithm, carried out as a result of their comparison with high- accuracy reference measurements. The study involved: • measurements carried out using LIDAR and a GNSS RTK receiver, and the synchronous recording of their results, 𝑛𝑒 𝑛𝑒 𝑛𝑒 𝑛𝑑 𝑑𝑜 𝑜𝑙 𝑖𝑑 Appl. Sci. 2019, 9, 3530 11 of 17 measurements carried out using LIDAR and a GNSS RTK receiver, and the synchronous recording of their results, Appl. Sci. 2019, 9, x FOR PEER REVIEW 11 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 11 of 17 the determination of the HDG, SOG and ROT, based on LIDAR measurement results, by the scan-matching method using both classic and modiﬁed versions of the ICP algorithm, • the determination of the HDG, SOG and ROT, based on LIDAR measurement results, by the • the determination of the HDG, SOG and ROT, based on LIDAR measurement results, by the an analysis comparing the HDG, SOG, and ROT with reference results of GNSS RTK measurements, sca scan n- -m ma atchi tchin ng g m metho ethod d usi usin ng b g bo oth th cl cla ass ssiic c a an nd d m mo od diif fiied ed v ver ersi sio on ns o s of f th the e ICP ICP a allgo gori rit th hm m,, based on the accuracy criterion. •• a an n a an na allys ysiis co s com mpa pari rin ng t g th he e HD HDG G,, S SO OG G,, a an nd d R ROT wi OT with th re ref fer erence ence re resul sults ts o of f G GNS NSS S R RTK TK measurements, based on the accuracy criterion. measurements, based on the accuracy criterion. The yacht port basin in Gdynia (Poland) was selected for the measurement area. Its boundaries include high, concrete wharves and a breakwater. Most of the time, good hydro-meteorological The yacht port basin in Gdynia (Poland) was selected for the measurement area. Its boundaries The yacht port basin in Gdynia (Poland) was selected for the measurement area. Its boundaries conditions prevail in the area (Figure 9). To carry out the measurements, a small USV (with a length iin ncl clude ude h hiigh gh, , co con ncr crete ete wh wha ar rv ves es a an nd d a a b bre rea akw kwa ater ter.. M Mo ost st o of f th the e ti tim me, e, go goo od d h hydr ydro o- -m mete eteo orol rolo ogi gica cal l of 1.62 cond m, itio width ns prev of ail0.40 in thm, e are and a (Fi agure draught 9). Toof carr 0.11 y out m) th equipped e measureme with nts,Scanse a small Sweep USV (wi Lidar th a l,engt a GNSS h conditions prevail in the area (Figure 9). To carry out the measurements, a small USV (with a length RTKo or f f eceiver 1 1..6 62 2 m m,, wi wi Leica d dth th o V of fiva 0 0..4 40 0 CS m m,15 , a an n[d d 48 a a], d dra and rau ugh gh an t t o oon-boar f f 0 0..1 11 1 m m) ) d equ equ computer iippe pped d wi wi con th th S S nected ca can nse se S Sto wee wee them p p L Liid d with a ar, r, a a G G a NS NS RS-422 S S R RTK TK /232C re rece ceiiv ver er L Le eiica ca Vi Viv va a CS CS 1 15 5 [ [4 48 8] ],, a an nd d a an n o on n- -b bo oa ard rd co com mpute puter r co con nn nec ected ted to to th them em wi with th a a R RS S- -4 42 22 2//2 23 32 2C C conversion cable [49], were used to record the measurement results (Figure 10). The measurements conversion cable [49], were used to record the measurement results (Figure 10). The measurements conversion cable [49], were used to record the measurement results (Figure 10). The measurements were carried out on a USV sailing with a speed of approximately 1.2 (2.4 kts) over a trajectory m m were carried out on a USV sailing with a speed of approximately 1.2 ( (2.4 kts) ) over a trajectory of were carried out on a USV sailing with a speed of approximately 1.2 2.4 kts over a trajectory of of approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer s s approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer wirelessly connected to AUSV (autonomous unmanned surface vehicle, see Figure 11.). While the wi wire rellessl essly y c co on nn nec ected ted to to A AU US SV V ( (a auto uton no om mo ous us u un nm ma an nn ned ed surf surfa ace ce v ve eh hiicl cle e,, see see F Fig igure ure 11 11..) ).. Wh Whiille e th the e USV was sailing, the on-board computer synchronously recorded (when generating a full scan using USV was sailing, the on-board computer synchronously recorded (when generating a full scan using USV was sailing, the on-board computer synchronously recorded (when generating a full scan using LIDAR) the measurement results making up the so-called data sets containing: a LIDAR scan (in the L LIDAR) IDAR) th the e m me ea as sure urem men ent r t resul esults ts m ma aki kin ng up g up th the e so so- -ca calllled ed d da ata ta sets co sets con nta taiin niin ng: g: a a L LID IDA AR R sc sca an n ( (iin n th the e form of a measurement set Z = (r , ), ::: , (r , ) ), NMEA messages with the position coordinates n n 1 1 f fo orm rm o of f a a m mea easure surem ment set ent set ℤ ℤ = = { {( (𝑟 𝑟 ,, ) ),, … … ,, ( (𝑟 𝑟 ,, ) )} }) ),, NM NME EA A m messa essage ges s wi with th t th he e po posi sit tiio on n coo coord rdiin na ates tes 1 1 1 1 n n n n of SOG and COG (course over ground) from the GNSS RTK receiver [50]. 865 data sets were thus o of f S SOG OG a an nd d COG COG ( (c co ourse urse o ov ver er g gro roun und d) ) f fro rom m th the e G GNS NSS S R RTK TK re rec cei eiv ver er [ [5 50 0] ].. 8 86 65 5 d da at ta a sets sets were were th thu us s collected by recording each subsequent sets every 0.2 s on average, i.e., after a change in the USV co coll llec ected ted b by y re reco cord rdiin ng g ea each ch su subseq bseque uen nt t se sets ts e ev ver ery y 0 0..2 2 s s o on n a av ver era age, ge, ii..e. e.,, a af fter ter a a c ch ha an nge ge iin n th the e U US SV V position by approx. 0.25 m. position by approx. 0.25 m. position by approx. 0.25 m. Figure 9. The trajectory covered by the USV at the time of measurements. Figure 9. The trajectory covered by the USV at the time of measurements. Figure 9. The trajectory covered by the USV at the time of measurements. Fig Figure ure 1 10 0. . Th The e di dist str rib ibut utio ion n o of f m mea easur surin ing g in inst str rum umen entts s o on n tth he e unma unman nn ned ed sur surfa fac ce e v ve eh hicle icle ( (USV USV) ).. Figure 10. The distribution of measuring instruments on the unmanned surface vehicle (USV). Appl. Sci. 2019, 9, x FOR PEER REVIEW 12 of 17 Appl. Sci. 2019, 9, 3530 12 of 17 (a) (b) Figure 11. (a) USV during a collection data phase; (b) wireless control system. Figure 11. (a) USV during a collection data phase; (b) wireless control system. Analysis of the Accuracy of Determining the COG and SOG Using a Modiﬁed ICP Algorithm Analysis of the Accuracy of Determining the COG and SOG Using a Modified ICP Algorithm Table 1 presents statistical parameters (i.e., the mean value, standard deviation and the maximum Table 1 presents statistical parameters (i.e., the mean value, standard deviation and the value) which characterize the computational accuracy of each parameter. The following acronyms maximum value) which characterize the computational accuracy of each parameter. The following were used to describe ICP’s variants: K_ICP for algorithm without modiﬁcation, H_ICP for robust acronyms were used to describe ICP’s variants: K_ICP for algorithm without modification, H_ICP version based on Huber ’s function, HM_ICP for robust version based on Huber ’s function with mean for robust version based on Huber’s function, HM_ICP for robust version based on Huber’s function error weighting and HD_ICP for robust version based on Huber ’s function with directional error with mean error weighting and HD_ICP for robust version based on Huber’s function with weighting. The lower index ICP was used for values computed by ICP algorithm’s variants and lower directional error weighting. The lower index ICP was used for values computed by ICP algorithm’s index R was introduced to point the values surveyed with GNSS RTK receiver. variants and lower index R was introduced to point the values surveyed with GNSS RTK receiver. Table 1. Summary of comparative statistical parameters. Table 1. Summary of comparative statistical parameters. Mean Value Standard Deviation Maximum Value Mean Value Standard Deviation Maximum Value Method SOG SOG DCOG DCOG SOG SOG DCOG DCOG SOG SOG DCOG DCOG R R R R R R ICP ICP ICP ICP ICP ICP 𝐒𝐎𝐆 ∆𝐂𝐎𝐆 𝐒𝐎 𝐆 ∆𝐂𝐎𝐆 𝐒𝐎𝐆 ∆𝐂𝐎𝐆 (m/s) ( ) (m/s) ( ) (m/s) ( ) Method − 𝐒𝐎𝐆 − ∆𝐂𝐎𝐆 − 𝐒𝐎𝐆 − ∆𝐂𝐎𝐆 − 𝐒𝐎𝐆 − ∆𝐂𝐎𝐆 𝐑 𝐑 𝐑 𝐑 𝐑 𝐑 K_ICP 0.031 0.08 0.113 0.57 0.524 3.5 (m/s) (°) (m/s) (°) (m/s) (°) H_ICP 0.013 0.05 0.031 0.14 0.319 0.72 HM_ICP 0.012 0.03 0.031 0.13 0.185 0.48 K_ICP −0.031 0.08 0.113 0.57 −0.524 −3.5 HD_ICP 0.007 0.03 0.026 0.11 0.18 0.42 H_ICP −0.013 0.05 0.031 0.14 −0.319 0.72 HM_ICP −0.012 0.03 0.031 0.13 −0.185 −0.48 A comparison of the parameter values (obtained based on 865 collected data packets) listed in HD_ICP −0.007 0.03 0.026 0.11 −0.18 0.42 Table 1 shows that the HD_ICP algorithm was the best in terms of the accuracy of determining the USVs movement parameters; it is followed by HM_ICP, H_ICP and K_ICP. Figure 12 shows a graph of A comparison of the parameter values (obtained based on 865 collected data packets) listed in SOG_ICP-SOG_R dierences. Table 1 shows that the HD_ICP algorithm was the best in terms of the accuracy of determining the Figure 12 shows that the accuracy of determining the SOG using the K_ICP method signiﬁcantly USVs movement parameters; it is followed by HM_ICP, H_ICP and K_ICP. Figure 12 shows a graph decreases for scans in the intervals ofh220, 365i andh720, 825i. These scans dier signiﬁcantly from of SOG_ICP-SOG_R differences. each other in rotation angles and include a greater number of erroneous measurement results than the others. This is undoubtedly due to the turns made by the USV at that time. The accuracy of determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber ’s function. The HD_ICP method proved to be the best in determining the SOG in a signiﬁcant part of the graph course. This was also conﬁrmed by the lowest values of the mean error, equal to0.007 (0.014 kts) ( ) and of the standard deviation, equal to 0.026 0.05 kts presented in Table 1. 𝐈𝐂𝐏 𝐈𝐂𝐏 𝐈𝐂𝐏 𝐈𝐂𝐏 𝐈𝐂𝐏 𝐈𝐂𝐏 Appl. Sci. 2019, 9, x FOR PEER REVIEW 13 of 17 Appl. Sci. 2019, 9, 3530 13 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 13 of 17 Figure 12. A graph of SOG − SOG differences. ICP R Figure 12 shows that the accuracy of determining the SOG using the K_ICP method significantly decreases for scans in the intervals of ⟨220, 365⟩ and ⟨720, 825⟩. These scans differ significantly from each other in rotation angles and include a greater number of erroneous measurement results than the others. This is undoubtedly due to the turns made by the USV at that time. The accuracy of determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber’s function. The HD_ICP method proved to be the best in determining the SOG in a significant part of the graph course. This was also confirmed by the lowest values of the mean error, equal to m m −0.007 (−0.014 kts) and of the standard deviation, equal to 0.026 (0.05 kts) presented in Table Figure 12. A graph of SOG SOG dierences. s s ICP R Figure 12. A graph of SOG − SOG differences. ICP R The Figure histogram 12 shows repr tha esent t the ing accthe uracy frequency of determi of occurr ning th ence e SOG of SOG using th e SOG K_ICP di m et er hences, od sign pr if esented icantly The histogram representing the frequency of occurrence ICP of SOG R − SOG differences, ICP R in dec Figur reases e 13 fo ,r shows scans even in the mor inter e clearly vals of that ⟨220 in , 365 terms ⟩ aof nd the ⟨720 accuracy , 825⟩. of These determining scans difthe fer s SOG, ignifithe canHD_ICP tly from presented in Figure 13, shows even more clearly that in terms of the accuracy of determining the method each othis erbetter in rota than tionthe angl others. es andMost include of the a gre values ater of num SOG ber of er SOG roneodi us m erea ences, surement determined resultsusing than SOG, the HD_ICP method is better than the others. Most of ICP the values Rof SOG − SOG differences, ICP R the the HD_ICP others. T , HM_ICP his is un,d and oubte H_ICP dly dmethods, ue to the fall turn within s madthe e by interval the US of V a0.05~0.05. t that time. The accuracy of determined using the HD_ICP, HM_ICP, and H_ICP methods, fall within the interval of −0.05~0.05. determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber’s function. The HD_ICP method proved to be the best in determining the SOG in a significant part of the graph course. This was also confirmed by the lowest values of the mean error, equal to m m −0.007 (−0.014 kts) and of the standard deviation, equal to 0.026 (0.05 kts) presented in Table s s The histogram representing the frequency of occurrence of SOG − SOG differences, ICP R presented in Figure 13, shows even more clearly that in terms of the accuracy of determining the SOG, the HD_ICP method is better than the others. Most of the values of SOG − SOG differences, ICP R determined using the HD_ICP, HM_ICP, and H_ICP methods, fall within the interval of −0.05~0.05. Figure Figure 13. 13. H Histogram istogram r repr epre esenting senting t the he fr frequency equency of of occurr occurrence ence of of SOG SOG − SOG SOG di differ ere ences. nces. I ICP CP R R Figure 14 shows that the accuracy of determination of the DCOG using the K_ICP method Figure 14 shows that the accuracy of determination of the ΔCOG using the K_ICP method signiﬁcantly decreases, as in the case of the SOG, for scans in the intervals ofh220, 365i andh720, 825i. significantly decreases, as in the case of the SOG, for scans in the intervals of ⟨220, 365⟩ and The HD_ICP method also proved to be the best in determining the DCOG in this case. This was also ⟨720, 825⟩. The HD_ICP method also proved to be the best in determining the ΔCOG in this case. This conﬁrmed by the lowest values of the mean error, equal to 0.03 , and of the standard deviation, equal was also confirmed by the lowest values of the mean error, equal to 0.03°, and of the standard to 0.11 , presented in Table 1. Figure 15 shows a histogram representing the frequency of occurrence of deviation, equal to 0.11° , presented in Table 1. Figure 15 shows a histogram representing the DCOG DCOG increment dierences. frequency of occurrence of ΔCOG − ΔCOG increment differences. ICP R ICP R The histogram representing the frequency of the occurrence of dierences DCOG DCOG ICP R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the DCOG. Its advantage over the others can be clearly seen in the interval Figure 13. Histogram representing the frequency of occurrence of SOG − SOG differences. ICP R of dierences DCOG DCOG h0.05, 0.05i. Given that the DROT results are identical with the ICP R DCOG results, their presentation and analysis have been omitted. Dierences in the algorithms’ Figure 14 shows that the accuracy of determination of the ΔCOG using the K_ICP method accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Dierences between significantly decreases, as in the case of the SOG, for scans in the intervals of ⟨220, 365⟩ and HD_ICP and HM_ICP versions are barely visible, so they were omitted. ⟨ ⟩ 720, 825 . The HD_ICP method also proved to be the best in determining the ΔCOG in this case. This was also confirmed by the lowest values of the mean error, equal to 0.03°, and of the standard deviation, equal to 0.11° , presented in Table 1. Figure 15 shows a histogram representing the frequency of occurrence of ΔCOG − ΔCOG increment differences. ICP R 0.50 — 0.55 0.50 — 0.55 0.50 — 0.55 0.45 — 0.50 0.45 — 0.50 0.45 — 0.50 0.40 — 0.45 0.40 — 0.45 0.40 — 0.45 0.35 — 0.40 0.35 — 0.40 0.35 — 0.40 0.30 — 0.35 0.30 — 0.35 0.30 — 0.35 0.25 — 0.30 0.25 — 0.30 0.25 — 0.30 0.20 — 0.25 0.20 — 0.25 0.20 — 0.25 0.15 — 0.20 0.15 — 0.20 0.15 — 0.20 0.10 — 0.15 0.10 — 0.15 0.10 — 0.15 0.05 — 0.10 0.05 — 0.10 0.05 — 0.10 -0.05 — 0.05 -0.05 — 0.05 -0.05 — 0.05 -0.10 — -0.05 -0.10 — -0.05 -0.10 — -0.05 -0.15 — -0.10 -0.15 — -0.10 -0.15 — -0.10 -0.20 — -0.15 -0.20 — -0.15 -0.20 — -0.15 -0.25 — -0.20 -0.25 — -0.20 -0.25 — -0.20 -0.30 — -0.25 -0.30 — -0.25 -0.30 — -0.25 -0.35 — -0.30 -0.35 — -0.30 -0.35 — -0.30 -0.40 — -0.35 -0.40 — -0.35 -0.40 — -0.35 -0.45 — -0.40 -0.45 — -0.40 -0.45 — -0.40 -0.50 — -0.45 -0.50 — -0.45 -0.50 — -0.45 Appl. Sci. 2019, 9, x FOR PEER REVIEW 14 of 17 2.0 K_ICP H_ICP HM_ICP HD_ICP 1.0 0.0 -1.0 Appl. Sci. 2019, 9, 3530 14 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 14 of 17 Appl. Sci. 2019, 9, x FOR PEER REVIEW 14 of 17 -2.0 2.0 2.0 -3.0 K_ICP H_ICP HM_ICP HD_ICP K_ICP H_ICP HM_ICP HD_ICP 1.0 1.0 -4.0 1 50 100 150 200 250 300 350 400 450 500 550 600 650 700 750 800 850 Scan number 0.0 0.0 Figure 14. A graph of ΔCOG − ΔCOG increment differences. ICP R -1.0 -1.0 -2.0 1 -2 6.0 0 -3.0 1 -3 2.0 0 K_ICP H_ICP HM_ICP HD_ICP -4.0 -4.0 1 50 100 150 200 250 300 350 400 450 500 550 600 650 700 750 800 850 1 50 100 150 200 250 300 350 400 450 500 550 600 650 700 750 800 850 Scan number Scan number Figure 14. A graph of ΔCOG − ΔCOG increment differences. Figure 14. A graph of ΔCOG ICP − ΔCOG R increment differences. Figure 14. A graph of DCOG DCOG increment dierences. IICP CP R R Δ COG difference [º] K_ICP H_ICP HM_ICP HD_ICP K_ICP H_ICP HM_ICP HD_ICP Figure 15 60 . A histogram representing the frequency of occurrence of ΔCOG − ΔCOG increments. 60 ICP R The histogram representing the frequency of the occurrence of differences ΔCOG − ΔCOG ICP R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval ⟨ ⟩ of differences ΔCOG − ΔCOG −0.05˚, 0.05˚ . Given that the ΔROT results are identical with the ICP R ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms’ Δ COG difference [º] Δ COG difference [º] accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between Figure 15. A histogram representing the frequency of occurrence of ΔCOG − ΔCOG increments. HD_ICP Figure and 15 HM . A _ hICP istogv ra er m si ro epr ns a esen re tb in ag re th ly e v friequen sible, cso y o th f o ey ccurr were enc e om of itted ΔCOG . ICP − ΔCOG R increments. Figure 15. A histogram representing the frequency of occurrence of DCOGICP DCOG R increments. ICP R The histogram representing the frequency of the occurrence of differences ΔCOG − ΔCOG The histogram representing the frequency of the occurrence of differences ΔCOG ICP − ΔCOG R ICP R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval ⟨ ⟩ of differences ΔCOG − ΔCOG −0.05˚, 0.05˚ . Given that the ΔROT results are identical with the ⟨ ⟩ of differences ΔCOG ICP − ΔCOG R −0.05˚, 0.05˚ . Given that the ΔROT results are identical with the ICP R ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms’ ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms’ accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between HD_ICP and HM_ICP versions are barely visible, so they were omitted. HD_ICP and HM_ICP versions are barely visible, so they were omitted. (a) (b) Figure 16. (a) Grid map computed by ICP with proposed directional weighting—HD_ICP; (b) grid map computed standard H_ICP. 4. Conclusions As the study attempted to prove, an ICP algorithm that matches point clouds from a laser scanner can eectively generate USV movement parameters. The proposed weighting factor based on the mean error in the speciﬁed direction of the determination of the coordinates of the positions of (a) (b) (a) (b) Δ COG difference [º] Number of samp Δ le COG s difference [º] Number of samples Δ COG difference [º] Number of samples Appl. Sci. 2019, 9, 3530 15 of 17 scan points enables more realistic (corresponding to the actual movement) matching of subsequent maps of the surrounding environment. This helps to determine the USV movement parameters with a higher accuracy. This was conﬁrmed by the study results—inter alia the values of statistical indices in the form of mean SOG SOG = 0.007 (0.014 kts) and standard deviation ICP R SOG SOG = 0.0025 (0.05 kts) and DCOG DCOG = 0.03 and standard deviation ICP R ICP R DCOG DCOG = 0.11 . However, it should be noted that the authors’ original weighting factor ICP R should be used in combination with a robust criterion function to reduce measurements with gross errors (e.g., based on Huber ’s function), as only such a solution can be fully applicable. The indicated mean error values lead us to a generalized statement that the developed method allows measuring SOG and COG with the accuracy required by the IMO. It should be noted, however, that the test was carried out in conﬁned waters (harbors, channels, berths, marinas), beyond which the method would not be eective due to the low LIDAR range. All data used in publication is available at [51]. Author Contributions: Ł.M., methodology, investigation, writing-original draft preparation, writing-review and editing, K.N., supervision, formal analysis, validation and funding acquisition. Funding: The article has been funded by a statutory research on marine optical navigation systems conducted in Polish Naval Academy. Conﬂicts of Interest: The authors declare no conﬂict of interest. References 1. IMO. Resolution A.824(19) Adoption of Amendments to Performance Standards for Devices to Measure and Indicate Speed and Distance; IMO: London, UK, 2000. 2. IMO. Resolution MSC.116(73), Performance Standards for Marine Transmitting Heading Devices (thds); IMO: London, UK, 2000. 3. Naus, K.; Waz, M. Precision in Determining Ship Position using the Method of Comparing an Omnidirectional Map to a Visual Shoreline Image. J. Navig. 2016, 69, 391–413. [CrossRef] 4. Polvara, R.; Sharma, S.; Wan, J.; Manning, A.; Sutton, R. Obstacle Avoidance Approaches for Autonomous Navigation of Unmanned Surface Vehicles. J. Navig. 2018, 71, 241–256. [CrossRef] 5. Allen, C.H. The Seabots are Coming Here: Should they be Treated as ‘Vessels’? J. Navig. 2012, 65, 749–752. [CrossRef] 6. Nowak, A. The Proposal to “Snapshot” Raim Method for Gnss Vessel Receivers Working in Poor Space Segment Geometry. Pol. Marit. Res. 2015, 22, 3–8. [CrossRef] 7. Xie, P.; Petovello, M.G. Measuring GNSS Multipath Distributions in Urban Canyon Environments. IEEE Trans. Instrum. Meas. 2015, 64, 366–377. 8. Wang, Y.; Chen, X.; Liu, P. Statistical Multipath Model Based on Experimental GNSS Data in Static Urban Canyon Environment. Sensors 2018, 18, 1149. [CrossRef] 9. Shaﬁee, E.; Mosavi, M.R.; Moazedi, M. Detection of Spooﬁng Attack using Machine Learning based on Multi-Layer Neural Network in Single-Frequency GPS Receivers. J. Navig. 2018, 71, 169–188. [CrossRef] 10. Guo, R.; Sun, F.; Yuan, J. ICP based on Polar Point Matching with application to Graph-SLAM. In Proceedings of the 2009 International Conference on Mechatronics and Automation, Changchun, China, 9–12 August 2009; pp. 1122–1127. 11. Navarro, W.; Vélez, J.C.; Orﬁla, A. Estimation of sea state parameters using X-band marine radar technology in coastal areas. In Proceedings of the Sixth International Conference on Remote Sensing and Geoinformation of the Environment (RSCy2018), Paphos, Cyprus, 26–29 March 2018. 12. Mendes, E.; Koch, P.; Lacroix, S. ICP-based pose-graph SLAM. In Proceedings of the 2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), Lausanne, Switzerland, 23–27 October 2016; pp. 195–200. 13. Yang, J.; Li, H.; Jia, Y. Go-ICP: Solving 3D Registration Eciently and Globally Optimally. In Proceedings of the 2013 IEEE International Conference on Computer Vision, Sydney, Australia, 1–8 December 2013; pp. 1457–1464. 14. Zhang, Z. Iterative point matching for registration of free-form curves and surfaces. Int. J. Comput. Vis. 1994, 13, 119–152. [CrossRef] Appl. Sci. 2019, 9, 3530 16 of 17 15. Men, H.; Gebre, B.; Pochiraju, K. Color point cloud registration with 4D ICP algorithm. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 1511–1516. 16. Durrant-Whyte, H.; Bailey, T. Simultaneous localization and mapping: Part I. IEEE Robot. Autom. Mag. 2006, 13, 99–110. [CrossRef] 17. Moratuwage, M.D.P.; Wijesoma, W.S.; Kalyan, B.; Dong, J.F.; Namal Senarathne, P.G.C.; Hover, F.S.; Patrikalakis, N.M. Collaborative multi-vehicle localization and mapping in marine environments. In Proceedings of the OCEANS’10 IEEE SYDNEY, Sydney, Australia, 24–27 May 2010; pp. 1–6. 18. Ribas, D.; Ridao, P.; Neira, J. Underwater SLAM for Structured Environments Using an Imaging Sonar; Springer Tracts in Advanced Robotics Springer Berlin Heidelberg: Berlin/Heidelberg, Germany, 2010; ISBN 978-3-642-14039-6. 19. Nieto, J.; Bailey, T.; Nebot, E. Recursive scan-matching SLAM. Robot. Auton. Syst. 2007, 55, 39–49. [CrossRef] 20. Pomerleau, F.; Colas, F.; Siegwart, R.; Magnenat, S. Comparing ICP variants on real-world data sets: Open-source library and experimental protocol. Auton. Robot. 2013, 34, 133–148. [CrossRef] 21. Rusinkiewicz, S.; Levoy, M. Ecient variants of the ICP algorithm. In Proceedings of the Third International Conference on 3-D Digital Imaging and Modeling, Quebec City, QC, Canada, 28 May–1 June 2001; pp. 145–152. 22. Olson, E. Robust and Ecient Robotic Mapping. Ph.D. Thesis, Department of Electrical Engineering and Computer Science, MIT, Cambridge, MA, USA, 2008. 23. Konecny, J.; Prauzek, M.; Kromer, P.; Musilek, P. Novel Point-to-Point Scan Matching Algorithm Based on Cross-Correlation. Mob. Inf. Syst. 2016, 2016, 1–11. [CrossRef] 24. Besl, P.J.; McKay, N.D. A method for registration of 3-D shapes. IEEE Trans. Pattern Anal. Mach. Intell. 1992, 14, 239–256. [CrossRef] 25. Callmer, J.; Törnqvist, D.; Gustafsson, F.; Svensson, H.; Carlbom, P. Radar SLAM using visual features. EURASIP J. Adv. Signal Process. 2011, 2011, 71–82. [CrossRef] 26. Amigoni, F.; Gasparini, S.; Gimi, M. Scan matching without odometry information. In Proceedings of the First International Conference on Informatics in Control, Automation and Robotics, SciTePress—Science and Technology Publications, Setúbal, Portugal, 25–28 August 2004; pp. 349–352. 27. He, Y.; Liang, B.; Yang, J.; Li, S.; He, J. An Iterative Closest Points Algorithm for Registration of 3D Laser Scanner Point Clouds with Geometric Features. Sensors 2017, 17, 1862. [CrossRef] [PubMed] 28. Censi, A. An ICP variant using a point-to-line metric. In Proceedings of the 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, USA, 19–23 May 2008; pp. 19–25. 29. Segal, A.; Haehnel, D.; Thrun, S. Generalized-ICP. In Proceedings of the Robotics: Science and Systems V; Robotics: Science and Systems Foundation, Boston, MA, USA, 28 June–01 July 2009. 30. Wang, Y.; Xiong, R.; Li, Q. Em-based point to plane icp for 3d simultaneous localization and mapping. Int. J. Robot. Autom. 2013, 28. [CrossRef] 31. Godin, G.; Rioux, M.; Baribeau, R. Three-Dimensional Registration Using Range and Intensity Information; The International Society for Optical Engineering: Boston, MA, USA, 1994. 32. Marinov, A.; Zlateva, N.; Dimov, D.; Marinov, D. Weighted ICP Algorithm for Alignment of Stars from Scanned Astronomical Photographic Plates. Serdica J. Comput. Soﬁa Bulg. 2012, 6, 101–110. 33. Amamra, A.; Aouf, N.; Stuart, D.; Richardson, M. A recursive robust ﬁltering approach for 3D registration. SignalImage Video Process. 2016, 10, 835–842. [CrossRef] 34. Wang, R.; Geng, Z. WA-ICP algorithm for tackling ambiguous correspondence. In Proceedings of the 2015 3rd IAPR Asian Conference on Pattern Recognition (ACPR), Kuala Lumpur, Malaysia, 3–6 November 2015; pp. 76–80. 35. Naus, K.; Wa˛z, ˙ M. Ripping Radar Image with the Sea Map Using the Alignment Method 2009–2019; Logistyka, Institute of Logistics and Warehousing: Poznan, Poland, 2010. 36. Naus, K.; Wa˛z, ˙ M. Accuracy in ﬁxing ship’s positions by camera survey of bearings. Geod. Cartogr. 2011, 60, 61–73. [CrossRef] 37. Scansce. User ’s Manual and Technical Speciﬁcations. Available online: www.robotshop.com/media/ﬁles/ pdf2/sweep_user_manual.pdf (accessed on 1 February 2019). 38. Marden, S.; Guivant, J. Improving the Performance of ICP for Real-Time Applications using an Approximate Nearest Neighbour Search. In Proceedings of the Australasian Conference on Robotics and Automation, Wellington, New Zealand, 3–5 December 2012; pp. 3–5. Appl. Sci. 2019, 9, 3530 17 of 17 39. Bentley, J.L. Multidimensional binary search trees used for associative searching. Commun. ACM 1975, 18, 509–517. [CrossRef] 40. Greenspan, M.; Yurick, M. Approximate K-D tree search for ecient ICP. In Proceedings of the Fourth International Conference on 3-D Digital Imaging and Modeling, Ban, AB, Canada, 6–10 October 2003; pp. 442–448. 41. Jost, T.; Hugli, H. A multi-resolution ICP with heuristic closest point search for fast and robust 3D registration of range images. In Proceedings of the Fourth International Conference on 3-D Digital Imaging and Modeling, Ban, AB, Canada, 6–13 October 2003; pp. 427–433. 42. Eggert, D.; Dalyot, S. Octree-based simd strategy for icp registration and alignment of 3d point clouds. ISPRS Ann. Photogramm. Remote Sens. Spat. Inf. Sci. 2012, I–3, 105–110. [CrossRef] 43. Huber, P.J. Robust Estimation of a Location Parameter. In Breakthroughs in Statistics: Methodology and Distribution; Kotz, S., Johnson, N.L., Eds.; Springer New York: New York, NY, USA, 1992; pp. 492–518. ISBN 978-1-4612-4380-9. 44. Zhao, Q.; Han, X.H.; Chen, Y.W. A robust registration method using Huber ICP and low rank and sparse decomposition. In Proceedings of the 2015 Asia-Paciﬁc Signal and Information Processing Association Annual Summit and Conference (APSIPA), Hong Kong, China, 16–19 December 2015; pp. 744–752. 45. Zhllin, L. Robust surface matching for automated detection of local deformations using least-median- of-squares estimator. Photogramm. Eng. Remote Sens. 2001, 67, 1283–1292. 46. Masuda, T.; Sakaue, K.; Yokoya, N. Registration and integration of multiple range images for 3-D model construction. In Proceedings of the 13th International Conference on Pattern Recognition, Vienna, Austria, 25–29 August 1996; pp. 879–883. 47. Bergström, P.; Edlund, O. Robust registration of point sets using iteratively reweighted least squares. Comput. Optim. Appl. 2014, 58, 543–561. [CrossRef] 48. Leica Geosystems. Leica GS10/GS15 User Manual. Available online: http://www.surveyequipment.com/ PDFs/Leica_Viva_GS10_GS15_User_Manual.pdf (accessed on 1 February 2019). 49. IEC. Maritime Navigation and Radiocommunication Equipment and Systems—Digital Interfaces—Part 3: Multiple Talker and Multiple Listeners. High Speed Network Bus; IEC: Geneve, Switzerland, 2014. 50. NMEA. National Marine Electronics Association. Available online: https://www.nmea.org/content/nmea_ standards/v411.asp (accessed on 5 February 2019). 51. Data Repository. Available online: https://github.com/lukisp2/ICP_DATA_MARINA (accessed on 16 August 2019). © 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Journal

Applied Sciences – Multidisciplinary Digital Publishing Institute

Published: Aug 28, 2019

Get 20M+ Full-Text Papers For Less Than $1.50/day. Start a 14-Day Trial for You or Your Team.

Learn More →

Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters

Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters

Get 20M+ Full-Text Papers For Less Than $1.50/day. Start a 14-Day Trial for You or Your Team.

Learn More →

Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters

Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters

References (55)

Abstract

Journal

Recommended Articles

There are no references for this article.

Our policy towards the use of cookies