Intelligent Lead: A Novel HRI Sensor for Guide Robots

This paper addresses the introduction of a new Human Robot Interaction (HRI) sensor for guide robots. Guide robots for geriatric patients or the visually impaired should follow user's control command, keeping a certain desired distance allowing the user to work freely. Therefore, it is necessary to acquire control commands and a user's position on a real-time basis. We suggest a new sensor fusion system to achieve this objective and we will call this sensor the “intelligent lead”. The objective of the intelligent lead is to acquire a stable distance from the user to the robot, speed-control volume and turn-control volume, even when the robot platform with the intelligent lead is shaken on uneven ground. In this paper we explain a precise Extended Kalman Filter (EKF) procedure for this. The intelligent lead physically consists of a Kinect sensor, the serial linkage attached with eight rotary encoders, and an IMU (Inertial Measurement Unit) and their measurements are fused by the EKF. A mobile robot was designed to test the performance of the proposed sensor system. After installing the intelligent lead in the mobile robot, several tests are conducted to verify that the mobile robot with the intelligent lead is capable of achieving its goal points while maintaining the appropriate distance between the robot and the user. The results show that we can use the intelligent lead proposed in this paper as a new HRI sensor joined a joystick and a distance measure in the mobile environments such as the robot and the user are moving at the same time.


Introduction
There are approximately 285 million visually impaired people around the globe. Among them, 39 million people are totally blind. In addition, 90% of the visually handicapped live in developing countries [1]. In Korea, there are approximately 45,000 visually impaired people. However, they are provided with only 60~70 guide dogs. Due to the aging of the population, the numbers of geriatric patients who suffer from dementia or poor physical health are on the rise. It is therefore important to develop guide robots for the elderly as well as for the visually handicapped.
A number of studies have been performed to identify a means of providing information on self location and obstacle location for the visually impaired. In an RFID-based study [2,3], a robot cart converts the location information which has been received through the RFID into voice data. As a navigation tool, various sensors are attached to clothes and then the acquired environment information with these sensors is delivered to the visually impaired through other sensory organs [4,5]. Many robots such as GuideCane [6,7], RoJi [8] and ROVI [9] have been studied in the area of autonomous robots for the visually impaired. In the above studies ultrasonic sensors are attached to the twowheeled mobile robot as a means of detecting obstacles. The user uses the feedback force through a stick for avoiding obstacles when the steering wheel is acting. This becomes the cause that any ground bumps are directly transmitted to the user. In another study, eyeDog [10] is designed for the user to follow the robot with a dog leash. The shock to the robot is not transmitted to the user because of the flexibility of the leash, but the robot cannot acquire information about the relative location of the user and the robot.
Ultimately, our goal is to fabricate a human-friendly guide robot. Among the visually-impaired population, totally blind are only 15%. For this reason human-friendly guide robot which is in a cooperative relationship with the user, not just a common mobile robot which only follows control commands is more needed. This study addresses the fabrication of a new HRI sensor (Figure 1) with which a user's location and control volumes can be acquired at the same time. We will call this an HRI sensor the "intelligent lead". The intelligent lead proposed in this study and a 3D conceptual view about its application is shown in Figure 1. The objective of the intelligent lead is to acquire a stable distance from the user to the robot, speed-control volume and turn-control volume, even when the robot platform with the intelligent lead is shaken on uneven ground. In this paper we explain the precise Extended Kalman Filter (EKF) procedure for this. The intelligent lead physically consists of a Kinect sensor, the serial linkage attached with eight rotary encoders, and an Inertial Measurement Unit (IMU), and their measurements are fused by the EKF. Since a user swings his/her arms back and forth while walking, it is very difficult to locate the user's body using only his/her hand position information. A new sensor is needed to locate the user's body, or one of the sensors used to detect obstacles can be used as well.
In many previous studies, an ultrasonic sensor was often used as a means for detecting obstacles. Recently, many studies on how to detect and avoid obstacles with 2D laser scanners, Kinect sensors and vision sensors [11,12] have been conducted. A vision system is light and cheap as a passive sensor. Among others, the Kinect sensor has been used to recognize a 3D object or environment in the robot industry due to its significant price competitiveness. Compared to the SR-3000 which is a commercial 3D laser scanner, it is less accurate. Since it is much cheaper, however, the Kinect sensor has been used to locate a user's body in this study.
The intelligent lead is structured to fuse the data which has been acquired through the serial linkage, Kinect and IMU sensor with the EKF. This paper is organized as follows. Section 2 describes a configuration of the major hardware of the intelligent lead. Section 3 describes how to configure the serial linkage and an IMU integrator with the EKF, and identifies a process to obtain sensor output values from the user's joint positions. Section 4 contains an analysis of the results of two tests which were been performed after attaching the proposed intelligent lead to a mobile robot. Finally, Section 5 contains conclusions and plans for the future.  The mobile robot consists of a 2D laser scanner, a landmark detector and a servo driver module. The intelligent lead output is used for generation of steering angle in modified VFH+ algorithm. The serial linkage is connected with rotary encoders or absolute encoders consecutively. The encoders which are selected should be small and precise. In general, however, larger encoders are more precise. MAE2-A2 made by USDigital Company is used in this study ( Figure 3). The encoder is magnetic and an absolute encoder which provides PWM pulse with a 4,096 resolution at a 360° turn. Thus, 0.088° of accuracy per joint is found. In addition, the sampling rate of the PWM pulse is 250 Hz. The Kinect sensor was attached to the end of the serial linkage to measure a user's shoulder and torso position. The Kinect sensor acquires depth information by applying IR patterns to an object and measuring them with an IR sensor. Compared to a general camera, there is no interference of ambient light, and it also works at night. Usability at night is considered an important characteristic for human-friendly guide robots.

Sensor System Architecture
We acquired a user's shoulder and torso position from depth image with OpenNI library. If the base of the serial linkage is shaken because a robot passes over a rough surface or makes a dynamic motion, the vibration is directly transmitted in the coordinates on a user's hand. To compensate for the vibration of the robot's base frame, an IMU is installed in the area which is held by the user's hand. Accelerometer and gyroscope data are used in the time prediction procedures. For this, an MTi sensor made by Xsens Company with 100 Hz of sampling rate has been used.
The three sensors mentioned above have measurement errors. First, in the case of the low-price IMU, the drift of the gyroscope causes cumulative errors. We used the encoder connected in the serial linkage with small products to reduce weight. As the size of an encoder decreases, accuracy declines as well. Also, as the serial linkage gets longer, the amount of position and pose errors increases proportionally. The position and pose errors differ depending on assembly accuracy as well as the length of the link and the encoder accuracy itself.
The Kinect sensor can suffer from occlusions by smoke/fog or obstacles. In addition, the performance of the Kinect sensor decreases in a dynamic environment. Therefore, it is necessary to reduce error in the integration system through fusion instead of separately using these sensors. This kind of integration can improve accuracy and prevent output errors generated from sensor failures   (Table 1 ates paramet te ar velocity and linear acceleration, respectively. The symbol P H means a position data calculated from the EKF. In this section, a system dynamic nonlinear equation, system matrix, and an output matrix which are necessary to design the EKF to fuse both the serial linkage and the IMU are introduced.   Table 2 represents the related symbols used to explain the EKF. The linear speed equation of the hand is as follows: where "n" refers to the definition in the navigation coordinate frame, and "b" refers to the definition in the body coordinate frame. The navigation frame is the same as the base frame of the serial linkage attached to the robot. The navigation coordinate frame is placed at the pinhole of the depth camera of the Kinect sensor. The body coordinate frame is located at the center of the IMU sensor. For the convenience of calculation, we assume that the hand coordinate frame coincides with the body coordinate frame. In the Equation (4), v b is defined in the body coordinate frame. However, a position term, P n is a data in the navigation coordinate frame at the composition of the system state which is defined below. Therefore, DCM (Direct Cosine Matrix) is multiplied.
The linear acceleration equation is as follows: The quaternion equations of rotation velocity, which represent coordinate transformation from body coordinate frame to navigation coordinate frame, are expressed in a differential equation as shown below: The b n C used in Equations (4) and (5) can be stated with the normalized quaternion as shown below: To induce state space representation from the nonlinear Equations (4~6), state vector and output vector are defined as follows: Then, the system matrix and the output matrix are obtained as follows: We need to first initialize x 0 , P 0 , Q, R and then to calculate time prediction procedure and measurement update procedure consequently. First we acquire roll, pitch and yaw angles from MTi sensor and then we convert Euler angle to quaternion q 0 with these data. The covariance matrix P n0 is set to an initially calculated value from the serial linkage. And the initial value of v b is set to 0. From these values, the initial state and the initial covariance matrix are assigned as follows: The process noise matrix and measurement noise matrix were determined to avoid the divergence of the filter output by trial and error as follows: In the above Equation (12), I n×n represents n by n dimensional identity matrix. The time prediction procedure shown in the block diagram above can be stated as follows: (1) With the current state vector and IMU sensor data, update the system matrix from the Equations (9). (2) Calculate the derivative of state vector by using the equation, The measurement update procedure can also be stated as follows: (1) Determine the output matrix with the current quaternion from the Equation (9). The first 3-terms of the state vector, which are updated through the above procedures, are the coordinates of the hand position viewed from the base coordinate frame of the serial linkage. As mentioned previously, we assume that the base coordinate frame of the serial linkage is placed at the pinhole of the depth camera of the Kinect sensor.

Intelligent Lead Output Generation
The intelligent lead proposed in this study acquires a user's left shoulder position, right shoulder position and torso position from the Kinect sensor and hand position with Serial linkage and IMU on a real-time basis.
The intelligent lead has two buttons. One is the start button and another is the trim button. If the user pushes the trim button current output volumes are saved in trim buffers. The intelligent lead has a hand position fault checker block. If the distance from the shoulder to the hand is larger than the sum of the upper arm length and the lower arm length, than the output volumes become zero. This prevents unwanted generation of output volumes when the user loses hold of the handle. If the user's hand position is obtained from the output of the EKF, the user's control intention should be investigated by using this data and the user's right shoulder, left shoulder and torso position data which have been obtained from the Kinect sensor. Because the size of walking steps, the amount of back and forth arm swings and the amount of movements to the left and right differ from individual to individual, it is necessary to get information on changes of the user's arm-joints while walking.
When people walk with a dog, it is more common to move the hand to the left and right to point out directions instead of changing the angle of the wrist. In addition, many people push their hand forward and pull down backward to increase or decrease the velocity. Figure 5 describes the actions for generating commands of speed increase, speed decrease, left turn and right turn respectively.
We assume the user is a right-hander. Figure 6 shows frames, angles and distance defined from the joint positions. If the left shoulder position, P SL is (x 1 y 1 z 1 ) T , right shoulder position, P SR is (x 2 y 2 z 2 ) T and torso position, P T is(x 3 y 3 z 3 ) T , then the normal vector n of the plane that includes these three points can be calculated as follows: The angles defined above can be obtained using the law of cosine: If the height of the hand position is above the height of the torso position, we assume that the user has the intention to control the robot's direction or speed. In addition, if θ V is above a certain limit value, then we conclude that the user bent his/her waist. We fixed the limited value to 45 degrees in the following test. In these cases the intelligent lead output forced to set 0. This assumption prevents unwanted output generation. The intelligent lead has the trim button to set trim values for speed control and turn control. The default trim value is as follows: Finally we get intelligent lead output values for control command as follows: In Equations (19) and (20), O S and O T mean speed control value and turn control value respectively, P H (z) and P T (z) represent z-axis value of hand and torso position respectively. The distance from the user's body to the robot can be calculated simply with the mean of two shoulder positions and a torso position:   Table 3 lists the related symbols for generating intelligent lead output.

Mobile Robot for Testing
Many studies have proposed strategies for robots to complete their tasks without any collision with obstacles and/or other robots. Especially the methods using the potential field [13] and the Vector Field Histogram Plus (VFH+) algorithm [14,15] have given diverse and useful results. In this study, the robot direction and speed were controlled based on the VFH+ algorithm by using the obstacle information acquired from a 2D laser scanner and current location information acquired from an IR landmark detector. The size of the robot and local map were fixed to be 0.8 m and 3 × 3 meters in the VFH+ algorithm.
The HOKUYO UTM30-LX was used as a 2D laser scanner to detect obstacles. Even though UTM-30LX has measure range from −135° to 135°, the data measured from −90° to 90° was used in this test. Its specifications are listed in Table 4. Hagisonic's Stargazer was used as an IR landmark detector. We firmly affixed a total of 16 landmarks as IR reflective materials to the ceiling, whose height is 2.5 meters. Landmarks were aligned with a 1 meter interval horizontally. To determine the steering angle with the VFH+ algorithm, it is first necessary to create a local map with 2D laser scanner data. Then from the local map, we obtain an obstacle vector, a Primary Polar Histogram, a Binary Polar Histogram, and a Masked Polar Histogram, in that order. Steering angle vfh h is calculated based on the final Masked Polar Histogram. The desired steering angle is simply determined by the sum of h vfh and O T , and steering angle error value h e is calculated by the difference desired angle and current angle as follows: In the above equation, h c is the current direction which is measured from the IR landmark detector and O T is the desired differential direction which is generated from the intelligent lead. The position error, p e is calculated as follows: In the above equation, p d represents the desired distance from a user to a robot which is fixed at 0.8 meters in this test. The symbol O S is the desired differential speed which is generated from the intelligent lead output, and O D is the measured distance from the user to the robot with the intelligent lead. For the test, a two wheel-type mobile robot was designed. Mitsubishi's 400W AC servo motor, 10:1 bevel gearbox, 13-inch wheels and PC (Intel i7-2600 CPU @3.4 GHz) were used. Figure 8 pictures the mobile robot and the intelligent lead sensor fabricated for the test. Figure 9 below shows the captured screen from the monitoring program made for our testing. We expressed calculated positions on the upper left side in 3D using the OpenGL. In Figure 9, the four dots on the top represent the user's head position, left shoulder position, right shoulder position, and torso position which have been detected by the Kinect sensor, respectively. In addition, the dot in the middle refers to the hand location which has been detected through the EKF. The values which were calculated from the four positions were marked on the bottom left. On the right side of the display a CCD camera image for visual tracking is shown, and on the bottom side we show current robot positions calculated from SLAM and detected from the Stargazer landmark detector. The values calculated from the VFH+ algorithm are displayed and selection buttons for servo motor speed setting are arrayed.

Test Results
In the first test, the control volumes were measured by moving the handle of the intelligent lead horizontally while the robot is stopped. Figure 10 shows acquired positions on the 3D viewer when we generated speed increase, speed decrease, left turn and right turn commands respectively.  Figure 11 shows measured values when the hand position was shaking. We intended that θ V changes via the defined limit value, 45 degrees and the height of the hand position goes from lower to upper than that of the torso position, and then from upper to lower. As shown in the Figure 12, in the region of 200~250 time tick, the user bent down at the waist. Since the height of the hand was above than that of the torso position, as θ V was larger than 45 degrees the intelligent lead output was not generated. The distance from the user to the robot was always generated, regardless of θ V value and the height of the hand position.
In the second test, the robot was programmed to move to (1, 1.5), (1, 4.5), (4, 4.5) and (4, 1.5) points with avoiding obstacles twice by using VFH+ algorithm. We located obstacles at (2, 2.5), (2, 3.5), (3.5, 2.5) and (3.5, 3.5) points. The obstacles were cylinders with 0.5 meter diameter. Figure 13 shows robot track measured with the Stargazer. Because of the alignment and flatness errors of landmarks, it is shown that the robot path was cut off whenever detected landmarks are changing. Nevertheless the robot reached the desired destinations by avoiding obstacles.   When a robot is moving around following a user, it is difficult to use a joystick without a fixed joystick pad. The proposed intelligent lead can solve this problem. We show how to use intelligent lead as a joystick and a distance measure. In the third test, the ability of the robot equipped with the intelligent lead to reach a destination by avoiding obstacles was tested. The intention was for the robot to lead the users keeping 0.8 meter distance from the user. Our objective was to make a track twice by passing points at (1, 1.5), (1, 4.5), (4, 4.5) and (4, 1.5). We controlled the speed of the robot with the intelligent lead to make a first track within 120 seconds and then a second track within 80 seconds. And we generated turn control commands through the intelligent lead to arrive at the destinations and avoid obstacles. Figure 14  point and green points mean en route points or destination points, respectively. The result shows that we could reach the destination points keeping pre-defined distance from the robot. Output data from the intelligent lead during the test is shown in Figure 15. Mean distance between user and robot was 0.8101 meters. We confirmed that the proposed intelligent lead could be used as a new HRI joystick. Moreover, our results suggested that motion of humans could interact with robots using the proposed intelligent lead. During many tests, the OpenNI algorithm for searching skeleton fails occasionally at turning intervals. This is because the user gets out of the FOV due to the limited FOV. In future work, we need to convert the software to be able to solve this problem.

Conclusions
In this paper, we introduced the intelligent lead as a new HRI sensor. To detect the user's hand position, we made a serial linkage consisting of eight rotary encoders and then we constructed the intelligent lead with the Kinect sensor, the serial linkage and MTi sensor as an IMU. Their measurements were fused by an EKF. A mobile robot was designed to test the performance of the proposed intelligent lead and the related positions are monitored by calculating in 3D with the OpenGL. Several tests were conducted to verify that the mobile robot with the intelligent lead is capable of achieving its goal points while maintaining the appropriate distance between the robot and the user. It is shown that the intelligent lead acquires a stable distance from the user to the robot, speed-control volume and turn-control volume on a real-time basis. Moreover, our results suggested that the motion of humans could interact with the robot by the proposed intelligent lead in mobile environments such as those where the robot and the user are moving at the same time. During many tests, the OpenNI algorithm for searching skeleton failed occasionally at turning intervals for reasons of the limited FOV. In the future work, we need to convert the software to be able to solve this problem.