A Three-Photo-Detector Optical Sensor Accurately Localizes a Mobile Robot Indoors by Using Two Infrared Light-Emitting Diodes

Indoor positioning systems are facing to the demand of large-scale industrial applications in mobile robotics. It is still challenging to create an indoor positioning system that is easily embeddable, accurate, robust and power efficient. We constructed an easily embeddable, low-power optical sensor named InLock without lens to localize a mobile robot indoors moving at <inline-formula> <tex-math notation="LaTeX">$0.20~m/s$ </tex-math></inline-formula> with an accuracy inferior to <inline-formula> <tex-math notation="LaTeX">$10cm$ </tex-math></inline-formula> for the position and 0.1rad for the heading by using only three photo-detectors (PDs) and two infrared Light-Emitting Diodes (LEDs). (i) We modelled the optical sensor based on only three photo-detectors and two infrared LEDs by taking into account radiometric properties. (ii) We constructed the optical sensor by optimizing the geometry of the beacon and the receiver. (iii) We implemented and validated online estimation algorithms for an operating range at a height up to <inline-formula> <tex-math notation="LaTeX">$3m$ </tex-math></inline-formula> by using an extended Kalman filter and a complementary filter. Our results showed that modelling the optical sensor so that it takes into account radiometric properties and it optimizes the geometry of the beacon can enhance the accuracy of the indoor positioning system.


I. INTRODUCTION
Global Positioning System (GPS) makes outdoors tracking and navigation reliable and easily embeddable for realtime applications. However, in confined environments, GPS positioning and navigation are inaccurate due to the strong degradation of the satellite signals which are attenuated by clouds, walls and obstructions [1]. The attenuated signal provides an unsatisfactory accuracy of localization that led to the development of Indoor Positioning Systems (IPS). There are two classes of localization scheme using PD or image sensor [2]. Accurate IPS presents multiple challenges such as risk of collision, variations on lighting conditions, congestion of the building infrastructure and limitations on embedded computer resources. How can we model and construct an help of ceiling LED lamps acting as beacons [12]. How can we reach the standard level of accuracy of 20cm for the indoor localization of the industrial Internet of Things by using a LED-based IPS?
The main challenge for the LED-based IPS is to estimate its own position from the optical signals received from the beacon. Any visible LED-based IPS aims at exploiting the received signal characteristics. Received Signal Strength (RSS), Time of Flight (TOF) or Angle of Arrival (AOA) by using photodiodes (PD) or cameras are combined with a positioning algorithm [13], [14]. In [15] and [16], trilateration and triangulation algorithms compute the position estimation.
For optical communications in free space under fog and smoke conditions, Ijaz et al. showed that near infrared light sources are the most robust wavelengths to link failure [17]. In [18], we proposed a minimalistic optical sensor without lens that estimates the relative position between the sensor and active markers using amplitude modulated infrared light. We showed that the sensor was able to estimate the position x and y at a distance of 2m with an accuracy as small as 2cm at a sampling rate of 100Hz. We implemented the sensor in a position feedback loop for indoor robotic applications in GPS-denied environment.
We aim at constructing a robust sensor for indoor position and heading estimations of a mobile robot for an operating range at a height up to 3m. We also aim at reaching a positioning accuracy inferior to 10cm limiting infrastructures modifications. We aim at using off-the-shelf PDs without lens instead of specific PDs as proposed in [19]. The IPS aims at accurately estimating the pose (position and orientation) of a robot in a set of critical areas such as near edges, near entrance and exit of corridors using a fixed beacon and a embedded receiver while the repeated unit cells of LEDs define the visual light positioning system in [20], [21].
In this paper, we modelled an optical sensor without lens based on only three PDs and two infrared LEDs by taking into account radiometric properties. We constructed the optical sensor by optimizing the geometry of the beacon and the receiver. Finally, we implemented and validated online estimation algorithms for an operating range at a height up to 3m by using an extended Kalman filter and a complementary filter.

II. SYSTEM OVERVIEW
We constructed an optical sensor without lens that we called InLock by optimizing the geometry of the beacon and the receiver.
The system overview is presented in Fig. 1. Figure 1a) describes the system configuration with one beacon. The beacon is composed of two infrared LEDs: LED 1 and LED 2. Lens can introduce strong distortions on the LED emission patterns that would add complexity to the overall system, as seen in [22]. To simplify the system, instead of using a lens, each LED was placed behind an optical diffuser. Optical diffuser is cheaper than lens, simpler to use, and homogenize the emission pattern to a smooth Gaussian emission pattern  with useful mathematical properties. The latter allowed us to model the system as detailed in appendix B. The LEDs emit a modulated infrared signal at two distinct frequencies f 1 = 5kHz and f 2 = 17kHz. The receiver is composed of three photodiodes PD A , PD B and PD C organized in a plane right-angle triangle and mounted on the commercial mobile robot TurtleBot3. Figure 1b) presents the mobile robot in top view. The vehicle's body frame {B} is shown in red and the earth frame of reference {E} is shown in black. The velocity in the x-direction is v and the vehicle's heading is φ. The vehicle's velocity in {E} is (v cos(φ), v sin(φ)).

A. INFRARED LIGHT BEACON
The beacon is composed of two infrared LEDs as presented in Fig. 2. The LEDs flicker at the frequencies f 1 = 5kHz and f 2 = 17kHz. Each LED is oriented in a specific direction of emission. The angle of emission of each LED provides mathematical properties for the algorithm of position and heading estimations.

B. INFRARED LIGHT RECEIVER
The infrared light receiver is composed of only three pixels without optics as shown on Fig. 3. It measures the demodulated infrared signals. It provides an analog signal which is the input for the digital processing.

III. SYSTEM MODEL
We modelled an optical sensor based on only three PDs and two infrared LEDs by taking into account radiometric properties.   Figure 4 gives an illustration in side view of the system for indoor positioning. The system model is composed of an infrared light beacon and a photodiode sensitive to the infrared light. It aims at modelling the voltage amplitude V PD,i taking to account radiometric properties.
The infrared LED i flickers at frequency f i just as in [20], [21], [23]. Each photodiode receives the infrared signal at each frequency. Consequently, the optical sensor composed of 3 photodiodes receives 6 signals (2 LEDs × 3 photodiodes). The signal is demodulated with an analog circuit and converted for digital processing. The voltage amplitude V PD,i is modelled by: We explain the expression of (1) as the product of three different gains: PD,i , models the variation over the distance D PD,i between the LED and the photodiode on the received signal strength,  , models the gain of the LED's radiation over the angle of emission θ E PD,i , iii cos(θ R PD,i ), models the gain of the photodiode with respect to θ R PD,i , the angle of reception. We used the photodiode OSRAM BPW 34 FAS whose the radiant sensitive area is 7.02mm 2 . The angular sensitivity of the photodiode is  modelled by a cosine-like angular sensitivity by taking to account the datasheet. The use of photo-detectors (PDs) is advantageous in terms of speed, sensitivity, energy consumption and system complexity [24]. The constant α i is defined by the input voltage and electronic gains of both the emitter and the receiver circuits, and we found α 1 = 15, α 2 = 13 in our experiments. The constant β i is defined by the optical diffuser's characteristic curve. β 1 = β 2 = 0.3 according to both the diffuser's datasheet and our experiments.
As shown in Fig. 5, the distance that separates the LEDs mounted on the beacon is 4cm. Therefore the distances from each LED to the photodiode PD are practically the same: Since the angles of reception depend on the distances, we assume they are the same: Taking these two approximations into account, we find the following relationship (see Appendix A): Assuming the beacon is mounted horizontally on the ceiling of a room, we define the direction of emission of each LED with the angles γ 1 , γ 2 and the vectors N are written in the earth frame of reference O E − {x E , y E , z E }: We define s 1 and c 1 as the sine and cosine of γ 1 . s 2 and c 2 are the sine and cosine of γ 2 . We also define in the body fixed frame attached to the robot L {B} = (x L y L z L ) T which is the position vector of the beacon and PD {B} = (x PD y PD z PD ) T which is the position vector of the photodiode PD. Considering the voltage ratio provided by (4) and the system's configuration, we introduce the variable r PD : The angle φ is the heading of the robot introduced in Fig. 1b).
Taking into account the geometry of the beacon, the expression of r PD becomes (see Appendix B for the mathematical development): where The expression of r PD gives: (i) a mathematical expression of the robot's position with x PD , y PD , z PD and the heading with c φ , s φ in (6), (ii) a mathematical model of the measurements. This model takes into account the geometry of the beacon (c 1 , s 1 , c 2 and s 2 ) and the radiometric properties with V PD,1 and V PD,2 in (7). λ PD stands for a variable of measurement which takes into account the optical properties of the beacon and the voltage amplitudes provided by the sensor.

IV. SENSOR CALIBRATION
We calibrated the sensor in order to estimate the robot's heading and the robot's position along x and y. From (7) and the expression of λ PD in (8), we defined the variable λ * PD = ln . Substituting the expression of λ PD given by (8) in (7), we write r PD as follows: where: Calibration result. We found the optimized gains k 0 , k 1 and k 2 such that the mathematical expression of r PD in (11) fits the measurements.
and α 1 = 15, α 1 = 13, β = 0.3. The robot is controlled using the motion capture system VICON. The position and the heading are recorded. When we plotted r PD from (6) versus λ * PD obtained from the measurements, we observed from Fig. 7, a linear shape. We decided to approximate r PD with a second order polynomial function of λ * PD : The calibration step aims at determining the values of the gains k 0 , k 1 and k 2 by using a least squares regression. The aim is to fit the expression of r PD in (11) to the expression of r PD in (6) given by the measured position and heading.

A. HEADING ESTIMATION
The use of LED1 and LED2 with chosen orientations allowed us to estimate the heading of the robot. Performing a calibration step for each photodiode A, B and C embedded in the receiver, we compute r A , r B and r C from (9). Moreover, the position coordinates are given in the robot's frame of reference. The positions of the photodiodes are known and correspond to their fixed position on the receiver. From (6), we can write the following equations for each photodiode: Computing the differences r A − r B and r A − r C , we write the following matrix equation: where: Since in our case of z A = z B = z C = 0, it gives: Equation (15) allows us to find both c φ z L and s φ z L . Since the height z L is a positive and constant value, we estimated the heading φ as follows: We can remark from (15) that we can estimate the heading even if the height is unknown. Once the heading calculated, one can use φ in (12) to estimate z L . In our case, we assumed that the height z L was known and constant. We found experimentally that this method gives a very good estimate of the heading.

B. ESTIMATION OF THE ROBOT's POSITION ALONG THE X -AXIS
The use of LED1 and LED2 with chosen orientations allowed us to estimate the robot's position along the x-axis.
We want to find the robot's position in the earth frame of reference {E} along the x-axis. We know that the relationship between the robot's position X = X {E} = (x y z) T in {E} and the beacon's position L = L {B} in the body frame of reference {B} is given by: We can also write: Developing the left side of the equation for photodiode A, we write: And developing the right side of (18): We can note from (20) that z = −z L . We can also note that: Using (21) and the definition of r PD in (6), we can write r A as follows:  Equation (22) gives an expression of x with respect r A . As described in (10), r A depends on the direction of emission of LED1 and LED2. In Fig. 8, we set γ 1 = 0 and γ 2 < 0. The use of LED1 and LED2 installed with two tilted angles γ 1 and γ 2 allowed us to estimate the robot's heading and the robot's position along the x-axis.
We can remark that y does not appear in (22) making impossible the estimation of the position along the y-axis. The reason is the use of only two LEDs instead of three. In future works, we will construct a beacon endowed with three infrared LEDs. The use of three infrared LEDs implies the construction of two new circuits boards for both the beacon and the receiver.

C. ESTIMATION OF THE ROBOT's POSITION ALONG THE Y -AXIS
LED1 allowed us to estimate the robot's position along the y-axis. We assume that the height z L is known and constant and the robot only rotates in yaw. We have θ E PD,1 = θ R PD,1 = θ 1 as presented in Fig. 9. Since D PD,1 = z L cos(θ 1 ) , substituting in (1), we write V PD,1 as follows: Assuming z L is known and constant, the voltage amplitude V PD,1 is a bijective function of θ 1 . We defined the function u PD (.) such that: Or, equivalently: since the mathematical expression of tan θ 1 is: To simplify the calculation, we defined the function g PD (.) such that: And we approximated the function g PD (.) by a second-order polynomial function. The aim of the calibration is to find by using a least squares regression, the optimal values of the gains b 0 , b 1 and b 2 as the following: For the sensor calibration, the distance between each photodiode and the beacon is accurately measured with the motion capture system. We used a trilateration-based algorithm to compute the robot's full position given by the following equation: And then, from (28) and (26): We programmed the robot to follow a reference trajectory. In Fig. 10, for each frequency f 1 = 5kHz and f 2 = 17kHz, we plotted the voltage amplitude measured for each PD used for the sensor calibration.
In Fig. 11, we plotted the results of the sensor calibration for the estimations of the heading and the robot's position along the x-axis. The plot in Fig. 11 a), b) and c) define the calibration functions useful for the estimations. For each photodidode A, B and C, the fitting curves obtained by using a least squares regression are linear.
The plot in Fig. 12 a), b) and c) allowed us to define the calibration functions for the estimation of the robot's position along the y-axis. For each photodiode A, B and C, the fitting curves obtained by using a least squares regression are second-order polynomial functions. Figure 13 presents the algorithm flowchart that allows us to calculate the heading and the positions in x and y of the mobile robot.

V. ALGORITHMS FOR HEADING AND POSITION ESTIMATIONS
We implemented online estimation algorithms for an operating range at a height up to 3m by using a complementary filter for the heading and an extended Kalman filter for the position.
Instead of using Angle of Arrival (AOA) based methods, we developed an algorithm based on (22) takes into account the radiometric properties of the optical diffuser and the geometry of the emitter. We present (i) the algorithm that  gives the heading estimation of the robot. A complementary filter optimizes the accuracy fusing the sensor measurements and the angular velocity provided by a gyroscopic sensor, (ii) the algorithm that estimates the positions in x and y using an Extended Kalman Filter (EKF).

A. INITIALIZATION
We initialized the algorithms by using the IMU and the optical sensor.

1) TILTING
First, we use use the IMU's accelerometer to calculate a first estimate of the robot's tilt. We define the orientation quaternion as q = q z ⊗ q xy where ⊗ is the Hamilton product. Here we note quaternions as 4x1 matrixes, where the fourth element is the quaternion's real part: Using the normalized accelerometer vector a = (a x a y a z ) T , we can use the methods defined in [25] to determine that:

2) HEADING
We use Equation (11) to get the values of r A , r B and r C . With the method described in Sec. IV, we initialize the value of the heading φ. We can define the heading quaternion as: The Hamilton product q = q z ⊗ q xy gives the initial orientation of the robot.

3) x ESTIMATE
Assuming that we know the (constant) height z L and that we have already calculated the orientation quaternion, we compute the yaw angle φ and use (22) to get three expressions for x: We initialize the first value of x taking the mean of these three values.

4) y ESTIMATE
From 18, we write the following equations: From 29, we can write the following equations for each photodiode: We find the unknown y in each equation and we compute the mean value.

B. HEADING ESTIMATION WITH A COMPLEMENTARY FILTER
We filtered the orientation quaternion q by using a complementary filter to prevent the robot's heading estimation from noise. The complementary filter that we implemented is taken from [25]. Instead of using the magnetometer to find the heading, we used the optical sensor InLock.

1) GYROSCOPE PREDICTION
The gyroscope gives a very precise measurement of the robot's angular velocities in every axis, defined here as ω = (ω x , ω y , ω z ). We defined the quaternion q ω as: Its time derivativeq k is given by: We used it to calculate the prediction quaternion: Then we normalized this prediction to make sure it continues to be a rotation quaternion. The pure integration of the gyroscope can lead to errors after some time, so after each prediction step we must correct it with the accelerometer and InLock sensor.

2) ACCELEROMETER CORRECTION
Assuming that the quaternion q acc corrects the prediction q pred to the real quaternion, we can write: In order to find q acc , we compute the modified accelerometer vector g: Then we define: We combine the data from both the accelerometer and the gyroscope using linear interpolation by (i) defining the gain α between [0, 1], (ii) updating the correction: We normalized q acc and defined the corrected quaternion as: In our experiments, α = 0.1 produced the best results.

3) InLock CORRECTION
We implemented the heading correction with the InLock sensor by using a correction quaternion q z . We applied first the accelerometer corrected prediction rotation to the positions of the photodiodes: The rotated positions A , B and C of the photodiodes result in a non-zero value for their z-component (z A = 0, z B = 0 and z C = 0). We write (13) as follows: where: Then we can calculate φ by first finding both c φ /z L and s φ /z L from Equation 45. Then, the heading correction quaternion is defined as: We defined the gain β between [0, 1] and defined the quaternion: The quaternion q z is normalized. The fully corrected quaternion is given by: In our experiments, β = 0.1 produced the best results.

C. POSITION ESTIMATION WITH AN EKF
We implemented an EKF to estimate the robot's position in x and y. VOLUME 8, 2020

1) MEASUREMENT VECTOR
In this case, the measurement vector Y is defined as: and tan 2 (θ PD,1 ) = g PD (V PD,1 ), where the functions f and g are defined as polynomial functions.

2) EKF STATE VECTOR AND MEASUREMENT VECTOR
We defined the state vector X as: 3) EKF STATE PREDICTION v x and v y are defined as the velocities of the robot in its own frame. We wrote a 1st order approximation for the state transitions as following: x where s φ and c φ were calculated from q as: s φ = 2(q w q z + q x q y ) Studying the kinematics of the commercial TurtleBot3, the state transition matrix is written as:

4) EKF OBSERVATION MODEL
We defined the observation model as: where the functions r PD and h PD are: 2 (55)

5) EKF OBSERVATION MATRIX
The observation matrix is defined as the Jacobian matrix of the observation vector in the state vector: Computing all the partial derivatives: We observed experimentally that the last three states in the measurement vector are very sensitive to small variations of height, pitch and roll. We decided to replace all the derivatives of the last three measurements in x, forcing the x estimate to only take into account the first three states in the measurement vector:

VI. RESULTS
We validated the online estimation algorithms for an operating range at a height up to 3m by using a complementary filter and an extended Kalman filter.

A. THE OPTICAL SENSOR IS EASILY EMBEDDABLE ON MOBILE ROBOTS AND CONSUMES LITTLE ENERGY
We previously proposed in [18] a minimalistic optical sensor in terms of small size and low power consumption (0.4W for the sensor and the analog demodulation board). The sensor enabled us to localize a mobile robot indoors (x and y positions) in operating range at a height of 2 meters. We asked whether we could construct an easily embeddable optical sensor for indoor localization of mobile robots (x, y and heading ϕ).
The mobile robot is a commercial TurtleBot3 Burger. It is equipped with the optical sensor as presented in Fig. 3. The position and heading estimations are compared to the reference using the motion capture system Vicon. We used ROS (Robot Operating System) to combined both the software (the algorithms for motion control) and the hardware components (the optical sensor equipped with the analog demodulation board, an IMU BNO055, an arduino Teensy 3.2, the TurtleBot3 motherboard).
The robot's velocity was about 0.20 m/s (the TurtleBot3 Burger's maximum velocity is around 0.22 m/s, according to the specifications). This value is coherent with the average velocity of the automatic guided vehicles used for industrial applications in warehouses (1 m/s) depending on their shape and their weight. Moreover, the InLock system is currently limited at a refresh rate of 33 Hz which is not an issue as regards of the robot's velocity. Since each LED emits alternatively during 15ms, the receiver has to wait for 30 ms to process the signal.
ROS is embedded in a raspberry pi 3 board which was mounted on the robot. We implemented using ROS the algorithms which regulated the robot's motion along a reference trajectory. We implemented the online algorithms (i) for the heading estimation by using a complementary filter, (ii) for the position estimation by using an Extended Kalman Filter detailed in Sec. V in an arduino Teensy 3.2 board (CPU 32 bit ARM Cortex-M4 at 72MHz).

B. THE OPTICAL SENSOR REACHED AN ACCURACY INFERIOR TO 0.1RAD FOR THE HEADING ESTIMATION OF THE MOBILE ROBOT
In [18], we succeeded in estimating the positions x and y of the mobile robot, but the heading was missing. We asked whether it was necessary to revise the geometry of the beacon and of the sensor to combine two flickering infrared LEDs. Two flickering infrared LEDs are placed in the beacon as presented in Fig. 2 and only three PDs are placed in the receiver. Figure 14 a) gives a comparison of the heading estimation (in red) to the actual heading of the robot (in black).   Figure 15 b) gives the mean error µ x = −0.036cm and the standard deviation is σ x = 1.1cm. This result enables us to state that the optical sensor can reach an accuracy very much lower than 10cm as targeted. Figure 16 a) presents a comparison of the position estimation along y-axis (in red) to the actual position of the robot versus time (in black). The estimate is closed to the actual position of the robot. Nevertheless, one can remark that the estimation differs from the reference at the lowest values. The estimation position along the y-axis is less accurate than the one along the x-axis because it is sensitive to the variations of height, pitch and roll. The reason is the use of only LED1 for the estimation of position. Figure 15 b) gives the mean error µ y = −2.1cm and the standard deviation is σ y = 3.2cm. This result enables us to state that the optical sensor can reach an accuracy inferior to 10cm as targeted. Table 1 VOLUME 8, 2020  gives a comparison of the position and angular performances with a non-exhaustive list of indoor localization schemes. With an accuracy inferior to 10cm for the position and 5 • for the heading in experiments, InLock can be considered of great interest for robotic applications.

VII. DISCUSSION
The main conclusion of this work is that we constructed an easily embeddable, low-power optical sensor without lens that localizes a mobile robot indoors with an accuracy inferior to 10cm by using only three PDs and two infrared LEDs. Our results showed that modelling the optical sensor so that it takes into account radiometric properties and it optimizes the geometry of the beacon can enhance the accuracy of the indoor positioning system.
In order to prove the robustness of the positioning system, we programmed the robot to successively repeat the same trajectory ten times. Figure 17 presents in 2D the actual trajectory X versus Y of the robot (in black). The estimated trajectory is plotted in red. The optical InLock sensor estimated the robot's position without drift.
As limitations, we noted in the results that the position estimation along the y-axis is less accurate than the one along the x-axis because of the sensor's sensitivity to the variations of height, pitch and roll. The sensor's sensitivity is due to the use of only one infrared LED for the estimation of the robot's position along the y-axis. In future work, we will construct a beacon composed of three infrared LED's in order to improve the accuracy of the position estimation. It will be necessary to construct analog modulation and demodulation boards.
One further limitation of InLock is the current refresh rate. Since each LED emits alternatively during 15ms, the receiver has to wait for 30 ms to process the signal. Since InLock aims at localizing faster robots in the future, the refresh rate will be increased with an improved design of the modulation board driving the LEDs.
As benefit of our work, we provided a model of the optical sensor taking into account radiometric properties.

APPENDIX A DERIVATION OF VOLTAGE RATIO RELATIONSHIP
Taking V PD,1 and V PD,2 the voltage values perceived by photodiode PD from LEDs 1 and 2, respectively, and taking the approximations of (2) and (3) into account: The relation between the angles of emission and the voltage ratio for the photodiode A is given as: The same deduction is used to find similar expressions for the other photodiodes.

APPENDIX B DERIVATION OF THE SYSTEM MODEL
We develop the mathematical equations that give the position and heading of the sensor InLock. We assume that the beacon is mounted horizontally on the ceiling. We model the equations in {B} which is the robot's frame of reference. We also assume that the robot can only rotate around the z axis by an angle φ from the beacon's frame of reference. Defining c φ = cos(φ) and s φ = sin(φ), we use the heading φ to write the following rotation matrix: And the inverse rotation matrix is given by: Defining the distance vector D PD as shown in Fig. 18, we write: Using the properties of the trigonometric functions, the left side of (64) can be written as follows: