Study of an Innovative Indoor Robotic Navigation Approach Based on Beacons and PSD

In this paper, innovative indoor navigation methods have been proposed to meet the challenges in robotic navigation systems. The general positioning methods for robotic navigation include vision-based approaches, WIFI beacons, infrared beacons, ultrasonic beacons, etc. However, the common problem with these methods is their inaccuracy. Especially, improving the precision of robotic positioning mechanisms is the key to indoor navigation systems. This paper proposes an approach that combines the external rotating beacon with an internal rotation of position sensitive devices (PSD) which are installed on the robot. While two infrared beams from an external beacon source are equally projected to both sides of the PSD, the robot’s position can be calculated precisely. The high performance and accurate results can be achieved by optimizing the rotation aligning time, dividing the working area, and compensating errors with information fusion. In comparison with other generic approaches, this proposed innovative approach requires less computing resources and is easier to implement due to its much lower complexity for the computing algorithms.


Introduction
Service robots are widely used in various situations.Route planning and selection will have a great impact on software performance and then the usability of the robots.In order to achieve the functional requirements of service robots, many navigation technologies have been researched.Indoor navigation approaches can be classified into map-based navigation and map-less navigation methods.Their core technology is based upon location identification.Many existing research efforts are based on artificial landmarks such as stripes and coloured objects, but the drawback of these approaches is that they heavily rely on specific tasks which are impractical in many occasions.
In essence, the location perception is essential for a robot to self-navigate in a room autonomously.The perceptive information can be collected from internal status, external signals or both.The internal perception information can be obtained from gyroscope, accelerator, odometer, etc. [1].The gyroscope can be used to infer the heading direction, but the disadvantage is the accumulation of errors which will eventually mislead the robot.The random drift, temperature variation, circuit noise, different kinds of errors will make the accurate hard to achieve [2].For MPU6050, the result is that even the robot is in a static state, the best performance of the gyro is shifted 5 degrees every ten minutes with a fullscale range of ±2000DPS.Moreover, while the robot is in moving the shift degree will be more serious, the consequence is that the gyroscope should be corrected periodically.To solve this problem, the digital compass was selected, although this will not accumulate the error thanks to the earth magnetic, nevertheless it is vulnerable to magnetic field interference, such as iron, mental furniture [3].As the robot has many motors and other execution components, their magnetism will have strong inference to the earth magnetism and will interfere the result of digital compass completely.
On the other hand, the odometer can be used to calculate the moving distance [4].Some researches on indoor navigation have assumed that the robot wheels will never slide and no error exists.Unfortunately, these preconditions will only exist in the virtual world.In reality, odometer can accumulate error quickly [5].In most cases the errors come from the difference in wheel radius, gear gap, and meanwhile the floor material also has a dominant influence.
The limitation of the internal sensor mentioned above can be solved by providing the periodically accumulation error correction thanks to exterior navigation signal.The external navigation facilities can be realized through sonar, laser range finder, beacons, virtual wall, vision analysis, RFID, etc.The precision of the measurement is the fundamental and central issue with the performance of the algorithm.According to the results of many research works, if the number of external navigation facilities was increased, the precision will also be improved.Nevertheless, the negative effect to production cost and easy installation will be an inevitable problem if the robot's target market is home applications.

FST 2016 IC
03 ( 2016To address the problem mentioned above, this paper proposed a method that uses only one beacon to afford precision indoor navigation.The main idea is to deploy two devices: beacon and receiver, where the beacon shoots two infrared beams alternatively to a onedimension position sensitive device (1D PSD) that mounted on the receiver.When a narrow beam illuminates the photosensitive strip of the PSD, two photocurrents, each inversely proportional to the distance of the spot from the far ends of the strip, are produced because the PSD outputs the centroid position of the spot.When two spots are on PSD simultaneously, the output position will be the centroid of two spots.Instead of these two beams be generated simultaneously, they should be generated alternatively to make two independent spots exist indifferent time.Then the position of two spots can further be processed by the method proposed in this paper.

Related research 2.1 PSD approach
The PSDis a precise distance detection sensor it can detect very small positional changes of the light spot on the active area,in contrast, CCD resolve distances limited by their pixel size and can lose microscopic spot information.Position Sensing Devices are divided into two families: the segmented PSDs and the lateral-effect PSD's.Lateral PSDs are continuous single element planar diffused photodiodes with no gaps or dead areas.These types of PSDs provide a direct readout of a light spot displacement across the entire sensing area.The PSD and its active area char, the general methodusetriangulation measurement system to determine the distance of an objectis illustrated in Fig. 1.The position conversation formula can be written as: (1) where Lx is the length of active area, Ix1 and Ix2 are currents for both electrodes respectively, x is the distance between light spot position and central of the active area.
The principle of PSD measurement is that the light from the light source is condensed by the lens and directed onto theobject.Light reflected from the object is condensed onto a one-dimensional position sensing device (PSD) by the receiving lens.If the position of the object (the distance to the measuring device) changes, the image formation positions on the PSD will differ and the balance of the two PSD outputs will change.If the two outputs are A and B, calculate A/(A + B) and use appropriate values for the span coefficient "k" and the offset "C" as shown below. (2)

The proposed PSD approach
To overcome the problem mentioned above as well asreduce the interference from the environment, we modulate the infrared light and beam the PSD directly.As overheat on the spot of the PSD will damage the device,the power of the infrared light should be properly controlled.Fig. 2. is the basic structure of our proposed indoor locating system,it consists of IR sender A and receiver B. On sending side A, around the circumference two laser diodes were installed,their angle is θ,Transmitter A shoots out a concentrated (by the lens) IR-light beam, this beam arrives B straightly and the light is focused on the PSD sensor.
The partially enlarged view of the transmitteris illustrated in right side of Fig. 2. Two laser diodes mounted alongside the circle of A, their angle is θ, they emit two laser beams that flash alternatively and they are perpendicular to the tangent line of the circle.A 1D PSD is mounted on the receiver side B, when the beam is projected onto the surface of PSD, we can calculate the distance of two light spots.Assume a virtual line between the center of two beams, if the virtual line is projected at the center of PSD, then it will perpendicular to the surface of the PSD which means two beam spots lies equally distance d to the center of PSD,then we can easily calculate the distance L=d/sin(θ/2).The θ should be selected carefully to satisfy the PSD size as well as the maximum and minimum detection length of L. Laser distance and azimuth detection based on PSD.The size of our selected PSD device is 1x15mm and the resolution is 0.1um, the response time is 0.8us.The PSD outputs track the motion of the 'centroid of power density' to an extremely high resolution and ultra-high linearity.The optical components used along with the sensor will add distortion, which can be implemented into the look-up table and thus minimized.To avoid interference from stray light the modulated light is used.The optical suit of A is rotated by a gear step-motor, the stepping angle of the step-motor is 5.625° and the gear reduction ratio is 64, so each step the angle rotation is 0.08789°.An optimal method is to utilize step-motor ) 9 control function,if the subdivision is 16 then the step can be divided by 16, hence the angle change will be 0.0055°.However, the actual output of the subdivision is influenced by electronic and mechanic parameter of the step-motor.For each step the distance between the intersection of two light beams of A and the dash circle will approximately be d=L*sin(0.08789).Here d<PW where PW is the width of PSD.As the width of the selected PSD is 1.5cm, the maximal L will be 9.77m.
The idea that measuring the distance and azimuth is: the sending side A and receiving side B were mutually rotating in an ad hoc way.A has a starting angle which is detected by a photo interrupter, meanwhile, both stepmotors of A and B keep rotating until two beams from A are projected on either side of the PSD of B and the two spots are equally distance to the center of PSD.The rotation angle of A is then recorded for azimuth.At the same time because line O1O2 perpendicular with PSD surface, the distance L can be get very easy.

Angular speed optimization
To make the system work properly the rotation angle of A and B should be dynamically adjusted to work in an opposed mode (through beam).As B is also installed on a step-motor,so it can rotate accordingly.Both A and B have a wireless module to make them communicate.
The adjusting formula for opposed mode is: ( where ω1 and ω2 are angle velocity of B and A, ψ1 and ψ2 are initial angle of B and A referenced by the same coordination system.The formula can be optimized, to improving processing time we use the following formula.
, n=1,3,5,… We need to decide the relationship between ω1-ω2 and adjusting time t and find out the optimal value for ω1 and ω2.The formula informs that ω1-ω2 and t are reciprocal function.The negative value of ω1-ω2 will lead to meaningless negative time,so we select the positive quadrant.
We can find from Fig. 3 if ω1-ω2→∞,t→0, if ω1-ω2→0,t→∞.While near the intersection point of the reciprocal functioncurve and its tangent line is about -45° (the best tradeoff between t and ω1-ω2 is t=ω1-ω2.It can calculate as: ,n is integer (5) Rules for the minimum adjust time t are: If ψ1>ψ2 and ψ1-ψ2<π then n=1, means the rotation angle needed will less than PI If ψ1>ψ2 and ψ1-ψ2>π then n=2,means the rotation angle needed will less than 2PI If ψ1=ψ2 then n=0 If ψ1<ψ2 and -2π<ψ1-ψ2<-π then n= -1,means the rotation angle will less than 3PI If ψ1<ψ2 and -π<ψ1-ψ2<0 then n= 0 When n was decided, ω1-ω2 was also determined.Let ω2 be a fixed angular speed,then ω1 is calculated by the above formula.By adjusting the speed of step-motor to ω1 that installed on the robot side B,the time for opposed adjusting for A and B will be minimal.While beacon A is installed in a fixed position ψ2, it is easy to determine the X axis of the global coordinate system as it will never change.On the contrary, B is hard to capture the X axis of the global coordinate system when the robot is in moving.To enable the ψ1 can be derived, a gyro can be added to determine the X axis of the global coordinate system.Since the gyro will accumulate error, the initial angle of gyro needs to be revised periodically.
From the above analysis, we know that the worst case for ω1-ω2 is , ω1=ω2+3.07 rad/s.This equation can be used when the start angle ψ1 of B is undetermined.Consider the factor of inertia, the step-motor will out of step if it stops suddenly.Therefore, the speed of stepmotor needs to be decreased before the opposing mode is reached.The next is the algorithm for location process.

Implementation
To fulfill the requirements mentioned above, thesystem described in Fig. 2 can be implemented in two separate devices: a beacon and a receiver.Fig. 4 illustrate the mechanical structure of the beacon.The left part is an internal symmetric shaped infrared component 114, two infrared diodeswere inserted into the cylinder-shaped hole,114a is one of the holes for accommodating diode while 114bis a very tiny long hole for the beam formalization, hence the structure can avoid scattering problem of two beams.The power of the diode should be carefully adjusted because the PSD saturation limits the spot brightness).The infrared component 114 that installed on the edge of a plate 111 is illustrated in right side of Fig. 4. Plate 111 is rotated by a step-motor 110.As step-motor will lose step in certain conditions, therefore periodic correction is necessary.Component 113 is a slot optical coupler, each time when 112 passes through the optical coupler will output a pulse.The pulse can be used by processor to correct the accumulated errorof thestep-motor.Fig. 5(a) is adetailedimplementation of the receiver device B in Fig. 2. 120 is aPSD component that installed on a plate 111, an optical filter 122 which used to reduce the background light level to avoid saturation effects of the PSD is mounted ahead, a lens 121 installed behind the filter, the spot diameter should be designed to about 200um,these are shown in Fig. 5(b).The height of 111 must be same as sender in Fig. 4. When the two beams send out from the sender and projected on the surface of PSD, the software will control the mutual tasks of the sender and receiver to make the two light spots equally projected on either side of PSD center, the algorithm will calculate the exact angle and distance of the receiver side.The beam is modulated and send alternatively,so the analog processing unit on the receiver side need to demodulate and then calculate the spot position and after filter procedure the final result will further processed by the position calculation algorithm.An important issue for successful implementation is error analysis.
Many aspects will influence the performance of this method.On this scenario, errors are sourcing from the beacon and receiver.The errors in the receiver side include PSD error and error of operational amplifier.PSD error will be influenced by voltage drift due to ambient temperature, stress of the sensor, light intensity, nonlinear, electron noise, etc.meanwhile, position resolution is limited by the signal to noise ration of the system.Compared with the receiver error, the beacon error is much more orders of magnitude.The reason is that the infrared diode is mounted on of the step-motor, the mechanic gap between the reduction gears will influence the spot greatly., so ∆θ is amplified by L. Because the deviation of the step-motor ∆θ is inevitable, even the step-motor will generate this error, such as step resolution, it means the above method couldn't work in reality.To solve this problem, this method needs to be revised.Assume the step-motor's step angle is σ, reduction gear ratio is k, the steps of around circle will be: The circle will be divide by N and the circumference is divided into arc segments, which the segment length will be (8) when k=64, σ=5.625°,L=10 meter, each arc length will be 0.15m.However, this is only in the ideal case.Fig. 6 is a sectional view of the detail description of the dotted line circle in Fig. 2, it illustrates how the gear clearance of the beacon's step-motor affect the system.In the idea condition, beams from beacon is draw in solid line and their intersection with circumference is in the middle of gray area.For example, position c0 and c1 are the intersection of the beams with a virtual circle in ideal case.Assume the error of gear clearance is ∆d, then the shift will +∆d or -∆d, which is the left or right edge of the gray sector.Their intersection with circumference will also change.Such as c0-,c0+,c1-,c1+.Because the characteristic of step-motor, this error will not accumulate.In account of the minimum and maximum shift of the step-motor,the small rectangle and large rectangle is draw on Fig. 6.The arc slices on the circumference can be classified into two categories: transition arc A0 and stop arc A1.The former is expressed with the arc bounded with small rectangle while the latter is bounded with outer rectangle.We define the white section on the disk as a transition area and the gray section on the disk as stop area where the receiver side may enter.According to the mechanic characteristic of step-motor, in arc A0 the light spot movement will much faster than arc A1.So the PSD mounted on the receiver will sense quicker light spot movement in A0 than A1.Assume the state S0 be corresponded with arc A0 and S1 with arc A1.

Linear speed determination based on response speed
Assume the angular speed of the beacon side step-motor is ω2, the linear speed from c0+ to c1-is, , where v must consistent with the response characteristic of PSD.The response speed of PSD is the time required for the generated carriers to be extracted as current by an external circuit.This is an important parameter when detecting a spot light traveling over the active surface at high speeds or using pulse-modulated light for subtracting the background light.FST 2016IC 03 (2016 5003 5003 voltage can reduce the rise time, but the minimum rise time is still needed to be satisfied.If we select 650nm infrared diode as a beam sender, when the inverse voltage is set in 5v, the rise time is approximately 1us.We can set the reverse voltage to 20v, and assume 2us be the worst case, according to the different infrared transmit diode adopted in beacon design.We can therefore compute the linear speed v by (9 where dl is the differential of length.We can omit the imperceptible speed variant of stepmotor when it's running at certain speed, the beacon's rotation can be regarded as constant speed, and therefore, the angular speed can be expressed as: (10) where t is determined by rise time, Lx is the length of PSD, L the distance from the beacon to the receiver.
For example, if given ω2=3.14 rad/s, L=10m, and Lx=2cm, then t will be 637us, which means the time that the light spot traveling over the active area of PSD will definitely satisfy its response speed.

Beacon direction tracing
When the system is initialized, the step-motor on the receiver side will rotated and eventually the surface of PSD will almost perpendicular with two light beams.After that the beacon will keep rotating, meanwhile the receiver which is mounted on the robot were moving in a certain pattern according to different application.The receiver also needs a gyroscope to trace the direction of the beacon, and meanwhile a periodic accumulation error elimination function will be called to remove accumulation error.By this method, the PSD on the receiver can be targeted by the beam easily.The following step is searching the beam, because in most cases the beam will only traveling through the PSD very quickly, so when the receiver received signal, it should inform the bean stop rotating and then move in the direction of the tangent line to find the beam again.

Navigation algorithm
The navigation algorithm is implemented both in sender and receiver, their parameters are mutual transferred by

Application scenario
The algorithm can be applied in cleaning robot.Fig. 8 is our prototype: the clean robot.The device described in Fig. 4 is realized in a quadrate box and a step-motor will be installed below it.The main PCB board consists an ARM processor STM32 and a FPGA chip.The main software is processed in arm processor while some signal processing function is implemented in FPGA which can lessen the workload impact on real time processing of the main processor.There is a large hole on top of the robot, a step-motor is installed there and the quadrate box is fixed on the axis of step-motor,see Fig. 8.

Conclusion
By adopting a step-motor driven PSD on the robot and an external infrared rotation beacon, and combining the algorithm described in this paper, the current robot distance and azimuth to the beacon can be calculated periodically.Therefore, the internal accumulation error can be eliminated.The pros are: it only needs one external beacon, and meanwhile the precision can be obtained without adopting expensive components.The con is: step-motorisvulnerableand will reduce system liability.

Figure 3 .
Figure 3. Angular difference with adjust time for opposed mode.

Figure 6 .
Figure 6.The error influence of step-motor.
Fig. 7 is the diagram of rise time vs. voltage, although increase the reverse,
WIFI.A and B will work collaboratively to go through with the algorithm.1 Device A and B rotate clockwise, angle speed of A equals with angle speed B plus 3.07 2 A detect a beam from B, slow down, inform B by wireless communication 3 After receive a notation from B, A slowdownand sending two beams alternatively 4 receiver moving along tangential line in Fig. 6 to find the beam again, and then slowly adjust the motor speed before the two beam spots projected equally onto either side of PSD center,stop rotating until v=v1=v2, where v1 or v2 is the voltage output for the left spot side or right spot side.5 Error compensation.According to v, compensate for the error of the operational amplifier and the PSD displacement, get the A/D result v' which is the output voltage of PSD.Find the lookup table and get the distance d according to v'. 6 When spots on either side of PSD are equal, calculate the distance and direction using formula , the direction β can be gained by counting the steps from X axis of the globe coordination system.7 B sending the parameter (L, β) to robot A 8 A stores the location parameter and revise its local position data.9 Local to globe coordination system transform.In the globe coordination system, position update for A is calculated as: P(x,y)=GP(A)*Rot(β)*L(X,Y), where P denotes globe coordinate system, GP denotes the globe position value x,y of A; Rot denotes a rotation transform from local angle β to globe angle; L denotes the coordination value for obstacle in the local coordination system of A, that is X,Y.When no obstacle exists, L(X,Y)=1.10 Goto step 1