A Rigorous Observation Model for the Risley Prism-Based Livox Mid-40 Lidar Sensor

Abstract

Modern lidar sensors are continuing to decrease in size, weight, and cost, but the demand for fast, abundant, and high-accuracy lidar observations is only increasing. The Livox Mid-40 lidar sensor was designed for use within sense-and-avoid navigation systems for autonomous vehicles, but has also found adoption within aerial mapping systems. In order to characterize the overall quality of the point clouds from the Mid-40 sensor and enable sensor calibration, a rigorous model of the sensor’s raw observations is needed. This paper presents the development of an angular observation model for the Mid-40 sensor, and its application within an extended Kalman filter that uses the sensor’s data to estimate the model’s operating parameters, systematic errors, and the instantaneous prism rotation angles for the Risley prism optical steering mechanism. The analysis suggests that the Mid-40’s angular observations are more accurate than the specifications provided by the manufacturer. Additionally, it is shown that the prism rotation angles can be used within a planar constrained least-squares adjustment to theoretically improve the accuracy of the angular observations of the Mid-40 sensor.

1. Introduction

Small form factor lidar sensors have become commonplace within numerous research areas and commercial applications, including autonomous vehicle navigation [1,2,3], mapping from unoccupied aerial systems (UAS), as known as remotely piloted aircraft systems (RPAS) [4,5,6], terrestrial-based personal mobile mapping systems [7,8], and recently for improving imaging and augmented reality capabilities on mobile devices [9,10]. Before integrating a lidar sensor into a remote sensing platform it is fundamental to understand the capabilities of the sensor, and this can only be achieved by a systematic evaluation of the accuracy, repeatability, and stability of the sensor [11]. Most commercially available, small form factor lidar sensors have already been systematically evaluated by researchers and the results published within literature (e.g., [11,12,13,14]). However, the Livox Mid-40 lidar sensor (Figure 1a)—which became commercially available in 2019—has only been partially evaluated to date, as its angular observations have not been rigorously evaluated. The principal topics of this study are the angular observations of the Mid-40 sensor and its suitability for use within UAS-based lidar mapping applications.

Unlike a multi-line lidar sensor (e.g., Velodyne VLP-16) which uses a mechanical rotating array of multiple laser transceivers, the Mid-40 uses only a single laser transceiver that is in a stationary position and orientation relative to the sensor’s own coordinate system, similar to most terrestrial laser scanners (TLS). However, unlike TLS which commonly use mirrors to reflect the laser beam, the beam of the Mid-40 is steered using a Risley prism optical mechanism consisting of two wedge-shaped glass prisms aligned sequentially in the direction of the optical scan axis (Figure 1b). Each prism can rotate about the optical scan axis resulting in the incident beam being refracted by the glass prisms. The emergent beam is deviated in a direction according to the relative orientation of the prisms with respect to each other [15].

Using a Risley prism mechanism allows for a wide range of unique scan patterns to be realized simply by adjusting the rotation rates and directions of each prism, see [15] for complete details. The scan pattern observed by the Mid-40 is commonly described as a rosette pattern and it occurs within a circular field of view (FoV) around the optical scan axis (Figure 2a). The rotation rates for the glass prisms inside the Mid-40 are set so the lidar observations are not repetitively taken along a single projected path within the FoV, rather the observation density within the FoV increases with time (Figure 2b). The observation density within the FoV is nonhomogeneous with a peak at the center, similar to the response of the human retina [16].

In comparison to the laborious manufacturing techniques used for mechanical multi-line lidar sensors, the Mid-40 is mass produced using a straightforward approach which enables the sensor to be sold at a lower cost [16]. In addition to the lower cost, the reported accuracy specifications and physical properties of the sensor make it an attractive option for UAS lidar mapping applications (

Table 1. Manufacturer’s Specifications for the Livox Mid-40 Lidar Sensor (Source: [17]).

Specification	Value
Laser Wavelength	905 nm
Laser Safety	Class 1 (IEC 60825-1:2014) eye safe
Detection Range	90 m @ 10% reflectivity 130 m @ 20% reflectivity 260 m @ 80% reflectivity
Field of View	38.4$°$ (Circular)
Range Precision	20 mm (1$\sigma$ @ 20 m)
Angular Accuracy	<0.1$°$
Beam Divergence	0.28$°$ (Vertical) × 0.03$°$ (Horizontal)
Point Rate	100,000 points/s
False Alarm Rate	<0.01%
Weight (with cable)	760 g
Dimensions	88 mm × 76 mm × 69 mm
Operating Temperatures	−20 $°$C to 65 $°$C
Water and Dust Rating	IP67
Price	$599 USD

). However, in order to fully understand the quality of the point clouds produced by the Mid-40, an assessment of the sensor’s accuracy specifications is needed. The primary research of this study is a rigorous examination of the systematic errors that affect the raw angular observations (i.e., azimuth angle and zenith angle) of the Livox Mid-40 lidar sensor for the purposes of accuracy assessment.

To date, two studies have been published that provide an initial assessment of the accuracies of the Mid-40. In Ortiz Arteaga et al. [18], the range and angular observation accuracies of the sensor were examined by comparing features within the collected point clouds (i.e., signalized targets and planes) to the same features within a more precise reference point cloud collected by a TLS. The study concluded that at longer standoff distances—40 m to 130 m—the Mid-40 achieved range measurements to an accuracy of ±20 mm. The angular accuracy was inferred by a trigonometric approximation of the ratio of the observed length error between known signalized targets over the standoff distance between the Mid-40 and the targets. The study concluded the angular accuracy of the Mid-40 to be 0.1$°$, but reported that an angular systematic effect may be present based on the appearance of concentric “ripples” of noise artifacts within some of the collected point clouds. The artifacts were believed to be correlated with the deflection angle of the emergent beam with respect to the optical scan axis and appeared within point clouds of planar surfaces that were observed from small standoff distances.

In Glennie and Hartzell [19], the range and angular observation accuracies were again examined by comparing the collected point cloud datasets to a more precise reference point cloud from a TLS. The analysis included 127 planar comparisons collected from standoff distances between 3 m to 35 m, and from incidence angles between approximately 5$°$ and 85$°$. The study concluded that the Mid-40 exhibited two classifications of range accuracies, ±8 mm for range observations greater than 20 m and ±21 mm for range observations less than 20 m. The angular accuracy was not explicitly analyzed, but a visual examination to identify any systematic effects correlated to the angular observations was conducted, see [19] for complete details. An attempt was made to duplicate the systematic “ripple effect” reported in [18], but no point cloud artifacts were found. The study concluded that no obvious systematic trends in the angular observations were observed.

Refs. [18,19] quantified the range accuracy of the Mid-40 to be in agreement with the manufacturer’s specification of ±20 mm and provided some evidence that the angular accuracy agrees with the manufacturer’s specification of <0.1$°$. However, both studies stated that the lack of information regarding the internal hardware configuration and operating principles of the Mid-40 restricted the ability to perform further analysis and uncover additional systematic errors.

This study presents the development of an angular observation model for the Livox Mid-40 lidar sensor. The observation model captures the physical prism parameters and systematic errors that affect the incident beam as it travels and refracts through the Risley prism steering mechanism and exits the sensor as the observable emergent beam. The observation model is used within an extended Kalman filter (EKF) to suitably represent the physical dynamics of the sensor and to statistically estimate the model parameters and the rotation angles for the individual prisms that correspond to the sequential azimuth and zenith observations. The filter’s inputs are the azimuth and zenith observations output directly by the Mid-40 sensor.

An initial analysis is performed using actual observations from a Mid-40 sensor to quantify the accuracy of the angular observation model and provide realistic estimates for the systematic errors. A secondary analysis is performed using simulated input observations—with a known amount of noise—to illustrate the filter’s ability to estimate the systematic errors and the prism rotation angles accurately and precisely. A final analysis is performed using simulated observations from an incorrectly calibrated sensor to a planar surface, to illustrate that the angular observations from an incorrectly calibrated sensor can be improved by using the range observations and applying a planar constraint within a least-squares adjustment. Lastly, a discussion on possible systematic errors within the range observation and the Cartesian coordinates representation of the observations is presented.

2. Materials and Methods

When discussing a conventional Risley prism steering mechanism there are four possible configurations for the internal arrangement of the two wedge-shaped glass prisms. Each prism has a perpendicular face (P) and an angled face (A) with respect to the optical scan axis. Therefore, the four configurations are PA-PA, PA-AP, AP-AP and AP-PA (Figure 3). Though each configuration follows the same generalized mathematical form for the direction of the emergent beam, selecting the correct configuration for the specific Risley prism mechanism being studied is important. Based on the information presented in [16], the Mid-40 Risley prism configuration is PA-AP.

2.1. Ideal Angular Observation Model

The angular observation model for a Risley prism steering mechanism starts with the unit vector of the incident beam and computes four successive refractions—using the 3D vector form of Snell’s law (Equation (10))—at the perpendicular and angled faces of each prism. Under ideal conditions, the incident beam, the planar normal on the perpendicular face of prism A, the axis of rotation for both prisms, and the planar normal on the perpendicular face of prism B are all collinear with the optical scan axis. The deflection angle between the perpendicular planar normal and the angled planar normal is equal to the wedge angle of the prism. At the zero position of the prisms, all four planar normals for the faces of the prisms lie in a common vertical plane (Figure 4). The prisms are geometrically identical and made of the same homogeneous medium with a precise refractive index. The prisms rotate at constant angular velocities and the rotation angles for each prism—with respect to the zero position—at the time of observation are expressed by:

$${\Omega }_A={\omega }_A\times t$$

(1)

$${\Omega }_B={\omega }_B\times t$$

(2)

where ${\Omega }_A$ and ${\Omega }_B$ represent the rotation angles for prism A and prism B at the time of observation $t$, respectively, and ${\omega }_A$ and ${\omega }_B$ represent the constant angular velocities for prism A and prism B, respectively.

The reference coordinate system for the ideal observation model is defined as a right-handed system with its origin at the intersection of the axis of rotation of prism B and the perpendicular face of prism B. The optical scan axis defines the X coordinate axis and the common vertical plane contains the orthogonal Z coordinate axis (Figure 4). The unit vectors for the direction of the incident beam (${\mathit{L}}_{\mathbf{0}}$), the planar normals for all four prism faces (${\mathit{N}}_{\mathbf{1}}$, ${\mathit{N}}_{\mathbf{2}}$, ${\mathit{N}}_{\mathbf{3}}$, ${\mathit{N}}_{\mathbf{4}}$), and the axes of rotation for the prisms (${\mathit{R}}_{\mathit{A}}$, ${\mathit{R}}_{\mathit{B}}$) are expressed by:

$${\mathit{L}}_{\mathbf{0}}={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$$

(3)

$${\mathit{N}}_{\mathbf{1}}={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$$

(4)

$${\mathit{N}}_{\mathbf{2}}={\left[\begin{array}{ccc}\cos \left(\alpha \right)& -\sin \left({\Omega }_A\right)\sin \left(\alpha \right)& \cos ({\Omega }_A)\sin \left(\alpha \right)\end{array}\right]}^T$$

(5)

$${\mathit{N}}_{\mathbf{3}}={\left[\begin{array}{ccc}\cos \left(\alpha \right)& \sin \left({\Omega }_B\right)\sin \left(\alpha \right)& -\cos \left({\Omega }_B\right)\sin \left(\alpha \right)\end{array}\right]}^T$$

(6)

$${\mathit{N}}_4={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$$

(7)

$${\mathit{R}}_{\mathit{A}}={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$$

(8)

$${\mathit{R}}_{\mathit{B}}={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$$

(9)

where $\alpha$ is the wedge angle of the prisms.

The 3D vector form of Snell’s law, modified from [20], is expressed by:

$${\mathit{L}}_{\mathit{i}+\mathbf{1}}=\frac{n_i}{n_{i+1}}\left[{\mathit{L}}_{\mathit{i}}-\left({\mathit{N}}_{\mathit{i}+\mathbf{1}}·{\mathit{L}}_{\mathit{i}}\right){\mathit{N}}_{\mathit{i}+\mathbf{1}}\right]+{\mathit{N}}_{\mathit{i}+\mathbf{1}}\sqrt{1-{\left(\frac{n_i}{n_{i+1}}\right)}^2\left[1-{\left({\mathit{N}}_{\mathit{i}+\mathbf{1}}·{\mathit{L}}_{\mathit{i}}\right)}^2\right]}$$

(10)

where ${\mathit{L}}_{\mathit{i}}$ and ${\mathit{L}}_{\mathit{i}+\mathbf{1}}$ (for i = 0, 1, 2, 3) represent the unit vectors for the direction of the initial beam and the refracted beam, respectively. ${\mathit{N}}_{\mathit{i}+\mathbf{1}}$ represents the unit vector for the direction of the planar normal on the refracting prism face, as expressed in Equations (4)–(7). $n_i$ and $n_{i+1}$ represent the refractive indices for the mediums (i.e., air or glass) on the initial and refracted sides of the prism face, respectively.

Once the unit vector for the direction of the emergent beam (${\mathit{L}}_{\mathbf{4}}$) has been determined, the equivalent azimuth and zenith angular observations—as defined with respect to the reference coordinate system of the Mid-40 (Figure 5)—are expressed by:

$$\mathsf{\varphi }=\arctan \left(\frac{L_{4y}}{L_{4x}}\right)$$

(11)

$$\mathsf{\theta }=\arccos \left(L_{4z}\right)$$

(12)

where $\mathsf{\varphi }$ and $\mathsf{\theta }$ are the azimuth and zenith angular observations, respectively, and $L_{4x}$, $L_{4y}$, $L_{4z}$ are the components of the unit vector for the emergent beam.

2.2. Practical Angular Observation Model

Several studies [20,21,22] have been published that discuss the error sources and their effects on the angular accuracy of Risley prism-based observations. Li et al. [20] state that the systematic error sources of a Risley prism mechanism include component errors and assembly errors. The component errors include the errors in the wedge angle and refractive index for each prism. The assembly errors include the tilt errors, axis of rotation alignment errors, and rotational position (i.e., encoder) errors for each prism, and also the incident beam direction error.

In reality, some of the assumptions made within the ideal angular observation model do not hold true. Within the practical angular observation model it is still assumed that the prisms are geometrically identical and made of the same homogeneous medium. However, the assumption that the planar normals for the faces of each prism all lie in a common vertical plane is no longer valid. Rather, for each prism a unique plane exists that contains the prism’s two planar normals and defines the so-called principal section of the prism (Figure 6). The zero position for each prism (i.e., ${\Omega }_A \mathrm{and} {\Omega }_B=0°$) is defined when its principal section is orientated vertically. Only by determining the principal section of each prism can the initial orientation of each prism be accurately calibrated and defined as the reference for calculating the prism rotation angle [20].

The reference coordinate system for the practical observation model is defined as a right-handed system with its origin at the intersection of the axis of rotation of prism B and perpendicular face of prism B. The axis of rotation of prism B (${\mathit{R}}_{\mathit{B}}$) defines the X coordinate axis and the vertically oriented principal section of prism B contains the orthogonal Z coordinate axis (Figure 6). Equation (9) from the ideal observation model still correctly defines the unit vector for ${\mathit{R}}_{\mathit{B}}$. However, the axis of rotation for prism A (${\mathit{R}}_{\mathit{A}}$) is no longer considered collinear with ${\mathit{R}}_{\mathit{B}}$, and is affected by the so-called bearing tilt error. As a result, the unit vector for ${\mathit{R}}_{\mathit{A}}$ is now expressed by:

$${\mathit{R}}_{\mathit{A}}=\left[\begin{array}{c}\cos \left(\mathsf{\Delta }{\varphi }_{R_A}\right)\cos \left(\mathsf{\Delta }{\theta }_{R_A}\right)\\ -\sin \left(\mathsf{\Delta }{\varphi }_{R_A}\right)\cos \left(\mathsf{\Delta }{\theta }_{R_A}\right)\\ \sin \left(\mathsf{\Delta }{\theta }_{R_A}\right)\end{array}\right]$$

(13)

where $\mathsf{\Delta }{\varphi }_{R_A}$ and $\mathsf{\Delta }{\theta }_{R_A}$ represent the bearing tilt error for the axis of rotation of prism A within the azimuth (XY) plane and corresponding zenith (⊥ XY) plane, respectively.

The unit vector for the direction of the incident beam (${\mathit{L}}_{\mathbf{0}}$) is also no longer considered collinear with ${\mathit{R}}_{\mathit{B}}$, and is affected by the so-called incident beam direction error. As a result, the unit vector for ${\mathit{L}}_{\mathbf{0}}$ is now expressed by:

$${\mathit{L}}_{\mathbf{0}}=\left[\begin{array}{c}\cos \left(\Delta {\varphi }_{L_0}\right)\cos \left(\Delta {\theta }_{L_0}\right)\\ -\sin \left(\Delta {\varphi }_{L_0}\right)\cos \left(\Delta {\theta }_{L_0}\right)\\ \sin \left(\Delta {\theta }_{L_0}\right)\end{array}\right]$$

(14)

where $\mathsf{\Delta }{\varphi }_{L_0}$ and $\mathsf{\Delta }{\theta }_{L_0}$ represent the incident beam direction error within the azimuth (XY) plane and corresponding zenith (⊥ XY) plane, respectively.

The unit vectors for the direction of the planar normals for all four prism faces are affected by the so-called prism tilt errors for each respective prism. As a result, the unit vectors at the zero position of each prism (${\mathit{N}}_{\mathbf{1}}^{\mathit{o}}$, ${\mathit{N}}_{\mathbf{2}}^{\mathit{o}}$, ${\mathit{N}}_{\mathbf{3}}^{\mathit{o}}$, ${\mathit{N}}_{\mathbf{4}}^{\mathit{o}}$) are expressed by:

$${\mathit{N}}_{\mathbf{1}}^{\mathit{o}}=\left[\begin{array}{c}\cos \left(\mathsf{\Delta }{\varphi }_{R_A}+\mathsf{\Delta }{\varphi }_A\right)\cos \left(\mathsf{\Delta }{\theta }_{R_A}+\mathsf{\Delta }{\theta }_A\right)\\ -\sin \left(\mathsf{\Delta }{\varphi }_{R_A}+\mathsf{\Delta }{\varphi }_A\right)\cos \left(\mathsf{\Delta }{\theta }_{R_A}+\mathsf{\Delta }{\theta }_A\right)\\ \sin \left(\mathsf{\Delta }{\theta }_{R_A}+\mathsf{\Delta }{\theta }_A\right)\end{array}\right]$$

(15)

$${\mathit{N}}_{\mathbf{2}}^{\mathit{o}}=\left[\begin{array}{c}\cos \left(\mathsf{\Delta }{\varphi }_{R_A}+\mathsf{\Delta }{\varphi }_A\right)\cos \left(\mathsf{\Delta }{\theta }_{R_A}+\mathsf{\Delta }{\theta }_A+\alpha \right)\\ -\sin \left(\mathsf{\Delta }{\varphi }_{R_A}+\mathsf{\Delta }{\varphi }_A\right)\cos \left(\mathsf{\Delta }{\theta }_{R_A}+\mathsf{\Delta }{\theta }_A+\alpha \right)\\ \sin \left(\mathsf{\Delta }{\theta }_{R_A}+\mathsf{\Delta }{\theta }_A+\alpha \right)\end{array}\right]$$

(16)

$${\mathit{N}}_{\mathbf{3}}^{\mathit{o}}=\left[\begin{array}{c}\cos \left(\mathsf{\Delta }{\varphi }_B\right)\cos \left(\mathsf{\Delta }{\theta }_B-\alpha \right)\\ -\sin \left(\mathsf{\Delta }{\varphi }_B\right)\cos \left(\mathsf{\Delta }{\theta }_B-\alpha \right)\\ \sin \left(\mathsf{\Delta }{\theta }_B-\alpha \right)\end{array}\right]$$

(17)

$${\mathit{N}}_{\mathbf{4}}^{\mathit{o}}=\left[\begin{array}{c}\cos \left(\mathsf{\Delta }{\varphi }_B\right)\cos \left(\mathsf{\Delta }{\theta }_B\right)\\ -\sin \left(\mathsf{\Delta }{\varphi }_B\right)\cos \left(\mathsf{\Delta }{\theta }_B\right)\\ \sin \left(\mathsf{\Delta }{\theta }_B\right)\end{array}\right]$$

(18)

where $\mathsf{\Delta }{\varphi }_A$ and $\mathsf{\Delta }{\theta }_A$ represent the tilt error within the azimuth (XY) plane and corresponding zenith (⊥ XY) plane for prism A, respectively, and $\mathsf{\Delta }{\varphi }_B$ and $\mathsf{\Delta }{\theta }_B$ represent the tilt error within the azimuth (XY) plane and corresponding zenith (⊥ XY) plane for prism B, respectively.

The unit vectors for the direction of the planar normals for all four prism faces at the time of observation (${\mathit{N}}_{\mathbf{1}}$, ${\mathit{N}}_{\mathbf{2}}$, ${\mathit{N}}_{\mathbf{3}}$, ${\mathit{N}}_{\mathbf{4}}$) can be determined by rotating the unit vectors at the zero position about the axis of rotation for each prism by the respective rotation angles ${\Omega }_A$ and ${\Omega }_B$. The rotation angles at the time of observation are defined by Equations (1) and (2). The unit vector for the axis of rotation for prism A (${\mathit{R}}_{\mathit{A}}$) is non-trivial, as expressed in Equation (13), because ${\mathit{R}}_{\mathit{A}}$ suffers from bearing tilt error. As a result, a quaternion representation is used to perform the rotation of ${\mathit{N}}_{\mathbf{1}}^{\mathit{o}}$ and ${\mathit{N}}_{\mathbf{2}}^{\mathit{o}}$ about ${\mathit{R}}_{\mathit{A}}$ by the rotation angle ${\Omega }_A$. The ${\mathit{N}}_{\mathbf{1}}$ and ${\mathit{N}}_{\mathbf{2}}$ unit vectors are now expressed by Equations (23) and (24).

$$q_{N_1^o}=0+N_{1x}^o i+N_{1y}^o j+N_{1z}^o k$$

(19)

$$q_{N_2^o}=0+N_{2x}^o i+N_{2y}^o j+N_{2z}^o k$$

(20)

$$q_{R_A}=\cos \left(\frac{{\Omega }_A}{2}\right)+\left(R_{Ax} i+R_{Ay} j+R_{Az} k\right)·\sin \left(\frac{{\Omega }_A}{2}\right)$$

(21)

$$q_{R_A}^{*}=\cos \left(\frac{{\Omega }_A}{2}\right)-\left(R_{Ax} i+R_{Ay} j+R_{Az} k\right)·\sin \left(\frac{{\Omega }_A}{2}\right)$$

(22)

$${\mathit{N}}_{\mathbf{1}}=q_{R_A}\otimes q_{N_1^o}\otimes q_{R_A}^{*}$$

(23)

$${\mathit{N}}_{\mathbf{2}}=q_{R_A}\otimes q_{N_2^o}\otimes q_{R_A}^{*}$$

(24)

where $q$ represents a unit quaternion and the terms i, j, k are the fundamental quaternion units, the terms $N_{1x}^o$, $N_{1y}^o$, $N_{1z}^o$ are the components of the unit vector ${\mathit{N}}_{\mathbf{1}}^{\mathit{o}}$, the terms $N_{2x}^o$, $N_{2y}^o$, $N_{2z}^o$ are the components of the unit vector ${\mathit{N}}_{\mathbf{2}}^{\mathit{o}}$, the terms $R_{Ax}$, $R_{Ay}$, $R_{Az}$ are the components of the unit vector ${\mathit{R}}_{\mathit{A}}$, and $\otimes$ represents the Hamilton product.

The axis of rotation for prism B (${\mathit{R}}_{\mathit{B}}$) defines the X axis of the reference coordinate system, therefore the ${\mathit{N}}_{\mathbf{3}}$ and ${\mathit{N}}_{\mathbf{4}}$ unit vectors are simply expressed by:

$${\mathit{N}}_{\mathbf{3}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left({\Omega }_B\right)& -\sin \left({\Omega }_B\right)\\ 0& \sin \left({\Omega }_B\right)& \cos \left({\Omega }_B\right)\end{array}\right]·{\mathit{N}}_{\mathbf{3}}^{\mathit{o}}$$

(25)

$${\mathit{N}}_{\mathbf{4}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left({\Omega }_B\right)& -\sin \left({\Omega }_B\right)\\ 0& \sin \left({\Omega }_B\right)& \cos \left({\Omega }_B\right)\end{array}\right]·{\mathit{N}}_{\mathbf{4}}^{\mathit{o}}$$

(26)

Once the unit vectors for the directions of the planar normals at the time of observation have been defined, the unit vector for the direction of the emergent beam (${\mathit{L}}_{\mathbf{4}}$) can be determined. Equations (14) and (23) are substituted into Equation (10) which determines the unit vector for the direction of the first refracted beam (${\mathit{L}}_{\mathbf{1}}$). Equation (10) is applied three more times by substituting in Equations (24)–(26) to determine the unit vectors ${\mathit{L}}_{\mathbf{2}}$, ${\mathit{L}}_{\mathbf{3}}$, and ${\mathit{L}}_{\mathbf{4}}$, respectively. Lastly, Equations (11) and (12) are used to convert the unit vector ${\mathit{L}}_{\mathbf{4}}$ into the equivalent azimuth and zenith angular observations.

Within the practical angular observation model a total of 13 model parameters are needed to determine the azimuth and zenith observations for the emergent beam at the time of observation. A time of observation equal to zero corresponds to the prisms being in their zero positions, as previously discussed. The model parameters are summarized in

Table 2. Parameters for the Practical Angular Observation Model.

Parameter Name	Symbol
Refractive Index of Air	$n_{air}$
Wedge Angle of Prisms	$\alpha$
Refractive Index of Prisms	$n_{prism}$
Angular Velocity of Prism A	${\omega }_A$
Angular Velocity of Prism B	${\omega }_B$
Horizontal Incident Beam Direction Error	$\mathsf{\Delta }{\varphi }_{L_0}$
Vertical Incident Beam Direction Error	$\mathsf{\Delta }{\theta }_{L_0}$
Horizontal Bearing Tilt Error for Prism A	$\mathsf{\Delta }{\varphi }_{R_A}$
Vertical Bearing Tilt Error for Prism A	$\mathsf{\Delta }{\theta }_{R_A}$
Horizontal Tilt Error for Prism A	$\mathsf{\Delta }{\varphi }_A$
Vertical Tilt Error for Prism A	$\mathsf{\Delta }{\theta }_A$
Horizontal Tilt Error for Prism B	$\mathsf{\Delta }{\varphi }_B$
Vertical Tilt Error for Prism B	$\mathsf{\Delta }{\theta }_B$

.

2.3. Observability of the Model Parameters

Liu et al. [16] state that the Mid-40 is composed of two identical prisms with a refractive index of 1.51, wedge angles of 18$°$, and angular velocities of $-$4664 revolutions per minute (RPM) and 7294 RPM for prisms A and B, respectively, following a right-hand rule for positive rotation around the X coordinate axis. However, the refractive index of air, the refractive index of the prisms, and the wedge angle of the prisms model parameters are perfectly correlated due to the use of Snell’s law within the observation model. As a result, the refractive index of air is set to a constant of 1.00 and the wedge angle of the prisms is set to a constant of 18$°$ within the observation model. The refractive index of the prisms model parameter is not fixed to a constant value to allow it to be optimized based on the sensor’s observations.

The azimuth and zenith angular observations—which are reported to a precision of 0.01$°$—are the raw outputs available from the Mid-40 sensor. It is important to analyze the sensitivity of the output observations with respect to changes in the model parameters, as well as the correlations between the model parameters, in order to understand the observability of the model. Using an extended Kalman filter—as described in the next section—analyses were performed to study the nature of these sensitivities and correlations. It was discovered that changes in all of the model parameters, except for the horizontal tilt error for prism A ($\mathsf{\Delta }{\varphi }_A$), were observable with respect to changes in the azimuth and zenith observations. The horizontal tilt error for prism A demonstrated to be relatively insensitive to changes in the azimuth and zenith observations and indicated a strong negative correlation (p = $-$0.99) with the computed estimates for the rotation angle for prism A (${\Omega }_A$) and the vertical tilt error for prism A model parameter ($\mathsf{\Delta }{\theta }_A$). As a result, the horizontal tilt error for prism A model parameter was assumed to be zero, and therefore had no effect on the practical observation model and was removed.

2.4. Estimation of the Model Parameters and Prism Rotation Angles Using a Kalman Filter

The practical angular observation model is utilized within an EKF to estimate the model parameters and the rotation angles of each prism at the observation times. The EKF is processed in both the forward and backward time directions, and the respective results are combined to produce an optimal smoothed estimate [23]. The EKF dynamics model for the system is expressed by:

$$\left[\begin{array}{c}\dot{n_{prism}}\\ \dot{{\omega }_A}\\ \dot{{\omega }_B}\\ \dot{\Delta {\varphi }_{L_0}}\\ \dot{\Delta {\theta }_{L_0}}\\ \dot{\Delta {\varphi }_{R_A}}\\ \dot{\Delta {\theta }_{R_A}}\\ \dot{\Delta {\theta }_A}\\ \dot{\Delta {\varphi }_B}\\ \dot{\Delta {\theta }_B}\\ \dot{{\Omega }_A}\\ \dot{{\Omega }_B}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ {\omega }_A\\ {\omega }_B\end{array}\right]+\left[\begin{array}{c}{w}_1\\ w_2\\ w_3\\ w_4\\ w_5\\ w_6\\ w_7\\ w_8\\ w_9\\ w_{10}\\ 0\\ 0\end{array}\right]$$

(27)

where $w_i=𝓝\left(0,{\sigma }_{w_i}^2\right)$ and represents the state transition noise.

The EKF measurement model for the system is highly non-linear and requires the use of Equations (13)–(26) and four successive applications of Equation (10) before the following measurement equations can be utilized:

$$\mathsf{\varphi }=\arctan \left(\frac{L_{4y}}{L_{4x}}\right)+v_1$$

(28)

$$\mathsf{\theta }=\arccos \left(L_{4z}\right)+v_2$$

(29)

where $v_i=𝓝\left(0,{\sigma }_{v_i}^2\right)$ and represents the measurement noise.

The EKF is initialized by examining the output azimuth and zenith observations and finding a time of observation that indicates the prisms are approximately in their zero positions. This occurs when the azimuth observation is approximately $0°$ and the zenith observation is approximately at its maximum value ($109.2°$). This time of observation is set equal to zero and all observations preceding this instance are disregarded and all proceeding observation times are adjusted accordingly. The EKF state variables are initialized as:

$$\left[\begin{array}{c}{n}_{prism}\\ {\omega }_A\\ {\omega }_B\\ \Delta {\varphi }_{L_0}\\ \Delta {\theta }_{L_0}\\ \Delta {\varphi }_{R_A}\\ \Delta {\theta }_{R_A}\\ \Delta {\theta }_A\\ \Delta {\varphi }_B\\ \Delta {\theta }_B\\ {\Omega }_A\\ {\Omega }_B\end{array}\right]=\left[\begin{array}{c}1.51\\ -\mathrm{27,984} °/\mathrm{s}\\ \mathrm{43,764} °/\mathrm{s}\\ 0°\\ 0°\\ 0°\\ 0°\\ 0°\\ 0°\\ 0°\\ 0°\\ 0°\end{array}\right]$$

(30)

It was discovered that a Mid-40 sensor—installed with v03.08.0000 firmware—operates using two different combinations of angular velocities for the prisms. The first combination uses nominal values of $-\mathrm{27,984} °/\mathrm{s}$ (—4664 RPM) and $\mathrm{43,764}$ $°/\mathrm{s}$ (7294 RPM) for ${\omega }_A$ and ${\omega }_B$, respectively, while the second combination uses nominal values of $-\mathrm{43,764} °/\mathrm{s}$ and $\mathrm{27,984}$ $°/\mathrm{s}$ for ${\omega }_A$ and ${\omega }_B$, respectively. The combinations alternate in occurrence when the sensor is power cycled, and it is speculated that this increases the lifespan of the brushless direct current (BLDC) motors that rotate the prisms. Therefore, the initial values for the 2nd and 3rd vector elements within Equation (30) must change when the second combination of angular velocities is experienced during data collection.

2.5. Estimation of the Model Parameters Using a Planar Constrained Least-Squares Adjustment

The practical angular observation model is also utilized within a non-linear least-squares adjustment to estimate the model parameters—excluding the angular velocities and the refractive index of the prisms—based on constraining the resulting Cartesian coordinates of the observed points to best-fit a planar surface. The point coordinates are calculated within the reference coordinate system of the Mid-40 using Equation (31):

$$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{c}{r}_p·\sin \left({\theta }_p\right)·\cos \left({\varphi }_p\right)\\ r_p·\sin \left({\theta }_p\right)·\sin \left({\varphi }_p\right)\\ r_p·\cos \left({\theta }_p\right)\end{array}\right]$$

(31)

where $x_p,y_p,z_p$ represent the Cartesian coordinates of point $p$, and $r_p,{\varphi }_p,{\theta }_p$ represent the range, azimuth, and zenith observations from a Mid-40 sensor to point $p$, respectively.

Before the adjustment begins, the input azimuth and zenith observations are processed using the EKF—as previously discussed—to estimate the current model parameters and the instantaneous rotation angles of the prisms that correspond to the input observations. Each iteration of the least-squares adjustment begins with computing new estimates for the azimuth and zenith angular observations for all points, using the practical angular observation model and the current estimates for the model parameters and the prism rotation angles. The point coordinates are then calculated and used within a singular value decomposition (SVD) problem to estimate the current best-fitting plane based on all the point coordinates. The perpendicular distances between the points and the best-fit plane are then calculated and used to define the minimization criteria for the least-squares adjustment. Equation (32) illustrates the unweighted least-squares solution:

$$\mathsf{\Delta }={\left(A^{T}A\right)}^{-1}{A}^T\epsilon$$

(32)

where $\mathsf{\Delta }$ represents the corrections to the current estimates for the model parameters, $A$ represents the Jacobian matrix containing the partial derivatives of the point-to-plane distances with respect to the model parameters, and $\epsilon$ represents the vector containing the negated values of the point-to-plane distances. The solution is repeated until convergence of the standard deviation of unit weight is achieved.

3. Results

3.1. Analysis of Real Mid-40 Angular Observations

Angular observations were collected from a stationary Mid-40 sensor and used within the EKF to estimate the observation model parameters, the prism rotation angles at the observation times, and the azimuth and zenith observation residuals. The estimated observation model parameters are considered to be an accurate representation of the unique calibration parameters of the sensor, as determined by the manufacturer and stored within the sensor’s non-volatile memory. The sensor was connected to a pulse-per-second (PPS) timing signal from a GNSS receiver to ensure accurate time observations were collected. Due to the fast 100 kHz observation rate of the Mid-40, only a small dataset containing 30 s of observations was used. To minimize the impact of any autocorrelations within the observations only one pair of azimuth and zenith observations for every one hundred collected pairs were used within the EKF (i.e., a 1 kHz observation rate was used). Figure 7 and Figure 8 illustrate the estimated mean values and sample distributions for the observation model parameters based on ~30,000 optimal smoothed estimates from the EKF (i.e., the output of each predict-measure-update iteration of the filter).

For a stationary sensor it was observed that the angular velocities of the prisms oscillate over time with approximately sinusoidal responses. The oscillations are speculated to be a result of the type, quality, and logic of the speed controllers used within the Mid-40’s electronics to control the BLDC motors that rotate each of the prisms [24]. Figure 9 illustrates the EKF smoothed estimates for the angular velocities of prisms A and B during the data collection period. The EKF also estimated the residuals for the azimuth and zenith observations and the respective distributions were approximately normal with means of $0°$ and standard deviations of $0.004°$ (Figure 10). The skewness and kurtosis values for the azimuth and zenith residual distributions were 0.00 and 0.64, and 0.01 and 0.59, respectively.

3.2. Analysis of Simulated Mid-40 Angular Observations

Using the practical angular observation model—with the same set of model parameters as estimated for the real Mid-40 sensor in the previous section—azimuth and zenith observations were computed using 30 s of 1 kHz prism rotation angles. Randomly sampled errors from a normal distribution with a mean of $0°$ and a standard deviation of $\pm 0.01°$ were added to the azimuth and zenith observations to produce a simulated dataset for a Mid-40 sensor. The simulated dataset was processed using the EKF to analyze if the model parameters, prism rotation angles, and observation errors could be correctly estimated. The results of the analysis demonstrated that all of the terms of interest could be accurately and precisely estimated.

Table 3. Comparison of simulated known and estimated terms.

Term of Interest	Simulated Known Value	EKF Estimated Value
Refractive Index of Prisms	$1.5090$	$1.5090\pm 0.0001$
Angular Velocity of Prism A	$-43,789.8°/\mathrm{s}$	$-\mathrm{43,789.8} °/\mathrm{s}\pm 2.2 °/\mathrm{s}$
Angular Velocity of Prism B	$\mathrm{27,997.8}°/\mathrm{s}$	$\mathrm{27,997.8} °/\mathrm{s}\pm 2.2 °/\mathrm{s}$
Horizontal Incident Beam Direction Error	$0.071°$	$0.071°\pm 0.002°$
Vertical Incident Beam Direction Error	$-0.385°$	$-0.385°\pm 0.002°$
Horizontal Bearing Tilt Error for Prism A	$0.011°$	$0.013°\pm 0.002°$
Vertical Bearing Tilt Error for Prism A	$0.008°$	$0.010°\pm 0.002°$
Vertical Tilt Error for Prism A	$0.090°$	$0.089°\pm 0.002°$
Horizontal Tilt Error for Prism B	$0.120°$	$0.117°\pm 0.002°$
Vertical Tilt Error for Prism B	$-0.383°$	$-0.384°\pm 0.002°$
Azimuth Observation Errors	$0°\pm 0.01°$	$0°\pm 0.008°$
Zenith Observation Errors	$0°\pm 0.01°$	$0°\pm 0.008°$
Rotation Angle Errors for Prism A	$0°$	$0°\pm 0.024°$
Rotation Angle Errors for Prism B	$0°$	$0°\pm 0.020°$

lists the known terms used within the simulation and their corresponding EKF estimates. Figure 11 illustrates the sample distributions for the estimated rotation angle residuals for prism A and prism B based on the simulated observations.

3.3. Analysis of Simulated Mid-40 Observations from an Incorrectly Calibrated Sensor

A final analysis was performed that simulated the scenario where the unchangeable manufacturer’s calibration values (i.e., model parameters) for a Mid-40 sensor were no longer correct. This may be the result of the sensor experiencing significant vibrations which cause physical changes to the alignment of the internal components within the sensor. In this case, any future angular observations made with the sensor would be inaccurate, but the range observations would be practically unaffected. The goal of the analysis was to investigate if range observations made to a planar surface could be utilized to estimate the correct calibration values for a sensor and therefore improve the accuracy of the angular observations.

Using the practical angular observation model along with 10 s of 1 kHz prism rotation angles, azimuth and zenith observations were computed using two different sets of model parameters. The first set of model parameters were the same as those estimated for the real Mid-40 sensor in Section 3.1 and defined the correct calibration. The second set of model parameters had all the model’s error parameters equal to $0°$ and represented the manufacturer’s unchangeable calibration values (i.e., the incorrect calibration). The correctly calibrated azimuth and zenith observations were used to compute simulated range observations from a Mid-40 sensor to a known planar surface. The planar surface was simulated to be 30 m away from the sensor with its planar normal being misaligned by $10°$ horizontally and $10°$ vertically with respect to the sensor’s optical scan axis. It was discovered that the observed planar surface needed to be non-perpendicular to the optical scan axis in order to break the rotational symmetry of the circular FoV. The simulated dataset consisted of the incorrectly computed azimuth and zenith observations along with the correctly computed range observations.

Before running the analysis, randomly sampled errors from normal distributions with means of $0°$ and 0 mm and standard deviations of $\pm 0.01°$ and $\pm$20 mm were added to the simulated azimuth and zenith observations, and the range observations, respectively. The root mean square error (RMSE) measures for the simulated azimuth and zenith observations were $\pm 0.077°$ and $\pm 0.396°$, respectively. The simulated dataset was processed using the planar constrained least-squares adjustment and the results demonstrated that the model parameters—and therefore the azimuth and zenith observations—could be estimated with improved accuracy and precision. The adjusted azimuth and zenith observations reported improved RMSE measures of $\pm 0.066°$ and $\pm 0.022°$, respectively. However, biases of $-0.065°$ and $-0.021°$ were still present within the angular observation residuals and are speculated to be a result of the range observation uncertainty limiting the achievable accuracy of the adjustment. Therefore, the 20 mm range uncertainty of the Mid-40 sensor defines the limit to the improvement of the angular observations using this method. Figure 12 illustrates the sample distributions for the original and adjusted angular observation residuals.

4. Discussion and Conclusions

Within the analyses of this study, real and simulated angular observations for a Mid-40 sensor were used to demonstrate that the practical angular observation model is accurate and precise, and enables rigorous uncertainty estimation of the angular observations from a Risley prism beam steering mechanism to be performed. The residuals between the model’s angular observations and those from an actual Mid-40 sensor were shown to be normally distributed with a mean of $0°$ and a standard deviation of $\pm 0.004°$. This suggests that the angular observations for the Mid-40 sensor are more accurate than the manufacturer’s specification of <$0.1°$. However, this finding does not take into consideration the angular uncertainty associated with the direction of the emergent beam due to beam divergence. The previous studies [18,19] provided inferred and qualitative evidence that the angular accuracy of the Mid-40 sensor was in agreement with the manufacturer’s specification, but also stated that the lack of information regarding the internal hardware configuration and operating principles of the sensor restricted further analysis. This study was performed to overcome these restrictions, by introducing an angular observation model and providing a rigorous accuracy assessment of the Mid-40’s angular observations.

Using the angular observation model within an extended Kalman filter allows for the azimuth and zenith observations to be used to estimate the model parameters as well as a set of lower-level observations, namely the rotation angles for each prism. The filter can accurately estimate the model parameters and the rotation angles for the prisms even when the prisms’ angular velocities are not constant. The inherent benefit of the prism rotation angles is that they are not affected by the systematic component errors and assembly errors of the sensor. By using the prism rotation angles and the corresponding range observations, an independent estimation of the sensor’s calibration values can be performed, and provides a method for improving the angular observations of an incorrectly calibrated Mid-40 sensor. However, the uncertainty in the range observations does set a limit to the achievable accuracy of the independent estimation.

In addition to the systematic errors that affected the angular observations, earlier research conducted as part of this study included analyzing errors within the range observation and the Cartesian coordinate representation of the azimuth, zenith, and range observations. No obvious systematic errors within the range observation could be identified; if any errors remain, they are likely non-geometric in nature and small in magnitude. Within the Cartesian coordinate representation of the observations a systematic error was discovered, and was a result of the emergent beam not coinciding with the origin of the sensor’s reference coordinate system. The error caused the Y and Z coordinates to be incorrect by upwards of 4.5 mm, see the Appendix A for details. Future work will include (1) conducting experiments using real Mid-40 angular and range observations to planar surfaces and estimating an improved set of calibration values for a sensor and quantifying that improved angular observations can be realized; (2) analyzing the angular observations from a triple Risley prism lidar sensor (e.g., Livox Horizon and Livox); and (3) studying the spatial distribution properties of a point cloud generated from a UAS lidar mapping system that utilizes a Mid-40 sensor.