sensors Article

A Simple Interface for 3D Position Estimation of a Mobile Robot with Single Camera Chun-Tang Chao, Ming-Hsuan Chung, Juing-Shian Chiou * and Chi-Jo Wang Department of Electrical Engineering, Southern Taiwan University of Science and Technology, 1, Nan-Tai St., Yongkang District, Tainan 71005, Taiwan; [email protected] (C.-T.C.); [email protected] (M.-H.C.); [email protected] (C.-J.W.) * Correspondence: [email protected]; Tel.: +886-9-1621152; Fax: +886-6-3010069 Academic Editor: Seung-Bok Choi Received: 20 January 2016; Accepted: 22 March 2016; Published: 25 March 2016

Abstract: In recent years, there has been an increase in the number of mobile robots controlled by a smart phone or tablet. This paper proposes a visual control interface for a mobile robot with a single camera to easily control the robot actions and estimate the 3D position of a target. In this proposal, the mobile robot employed an Arduino Yun as the core processor and was remote-controlled by a tablet with an Android operating system. In addition, the robot was fitted with a three-axis robotic arm for grasping. Both the real-time control signal and video transmission are transmitted via Wi-Fi. We show that with a properly calibrated camera and the proposed prototype procedures, the users can click on a desired position or object on the touchscreen and estimate its 3D coordinates in the real world by simple analytic geometry instead of a complicated algorithm. The results of the measurement verification demonstrates that this approach has great potential for mobile robots. Keywords: arduino; robot interface; single camera; 3D position

1. Introduction Robot technology has been successfully applied in many areas and is noted for its low cost, greater accuracy and reliability, suitability when there is a labor shortage, and its ability to provide entertainment. In particular robots can execute 3D (dirty, dangerous, and demeaning (or dull, difficult)) jobs for people. Compared with other modern autonomous robots, remote-controlled robots are much safer, which is a major concern in practical applications. Furthermore, increasingly more smartphone-controlled innovations are becoming available partly due to the adoption of smart phones as an everyday device by many people in highly-industrialized societies. However, controlling a mobile robot with a robotic arm by a smartphone or tablet involves a lot of motion actions which involve the visual control interface. This has caused many problems for engineers in the past. In order to improve the practicability of the mobile robot, real-time video transmission function should be added to the design, which allows the robot to operate out of sight of the user. Another crucial issue or function is the 3D position estimation of a target by a single camera. This paper is motivated by the issues mentioned above and aims to find applicable solutions for 3D position estimation with a mobile robot. The Arduino Yun [1], a microprocessor board with a built-in Wi-Fi module, was utilized to implement the proposed design. Arduino is an open-source electronics platform based on easy-to-use hardware and software launched in 2005. The goal of Arduino is to create easy-to-use and inexpensive control devices for projects [2,3], which is consistent with the requirements of the proposed system. Radio frequency (RF) wireless remote control is commonly used in commercially available products. Although lately there have been many debates over low-power Wi-Fi and RF (including ZigBee, 6LoWPAN, etc.) networks in the context of Internet of Things (IoT) applications and Sensors 2016, 16, 435; doi:10.3390/s16040435

www.mdpi.com/journal/sensors

Sensors2016, 2016,16, 16,435 435 Sensors

of 12 12 22 of

devices [4], since Wi-Fi is available on almost all smartphones, it is adopted as the wireless sensor devices [4], since Wi-Ficommands is availableand on real-time almost allvideo smartphones, it isin adopted as the system. wireless communication for the control transmission the proposed communication for the control commands and real-time video transmission in the proposed system. Reconstructing a target’s 3D location presents several problems. For example, an autonomous a target’s 3D location presents problems. For example, an autonomous robot Reconstructing generally uses stereo cameras and range sensorsseveral [5,6], but these devices are not commonly used robot generally uses stereo cameras and range sensors [5,6], but these devices are not commonly used in ordinary remote-controlled mobile robots. Over the past decade, many tracking methods, such as in ordinary remote-controlled mobile robots. Over the past decade, many tracking methods, such as Kalman filters [7], mean-shift [8], and Kanade-Lucas-Tomasi (KLT) algorithms [9] have been Kalman filters [7], mean-shift [8],However, and Kanade-Lucas-Tomasi (KLT) algorithms [9] have been developed in computer vision. these tracking algorithms are mostly based ondeveloped gradient in computer vision. However, these tracking algorithms are mostly based on gradient changes or changes or object features, which all require complicated computation or prior knowledge of the objectobjects. features, which alletrequire computation or prior target objects. target Karungaru al. [10]complicated applied a KLT tracker to obtain theknowledge distance toof thethe target point for Karungaru et al. [10] applied a KLT tracker to obtain the distance to the target point for a mobile robot a mobile robot equipped with single camera using motion stereo vision, but the tracking of the feature equipped with single camera using motion stereo vision, but the tracking of the feature points by points by optical flow analysis sometimes failed, further increasing the complexity of the approach. optical flow analysis sometimes failed, further localization increasing the complexity the approach. Another Another approach used a simple humanoid with a singleofcamera [11]. With this approach used a simple humanoid localization with a single camera [11]. With this approach, approach, only simple analytic geometry was applied to determine the real-world coordinates ofonly the simple analytic geometry was applied to determine the real-world coordinates of the robot using the robot using the goal posts in a soccer field as a landmark, making it practical and desirable for our goal posts in a soccer field as a landmark, making desirable for our system. However, system. However, in our application, no object canitbepractical used as and a landmark. in our application, no object can be used as a landmark. In this paper, we desire to develop a feasible approach to estimate the target’s 3D position for a Inrobot this paper, desire to develop a feasible approach to estimate the target’s for mobile with awe single camera. The camera needs to be properly calibrated and3D theposition odometer a mobile robot with a single camera. The camera needs to be properly calibrated and the odometer movement of the robot needs to be well designed to provide motion stereo vision. With the help of a movement ofand the arobot needs to be well designed to provide stereo vision. With the touchscreen user-friendly visual control interface, themotion 3D position calculation willhelp be of a touchscreen and a user-friendly visual control interface, the 3D position calculation will be implemented by simple analytic geometry and triangulation. This paper provides a practical implemented by simple analytic geometry and triangulation. This paper provides a practical reference reference for researchers attempting to design a visual remote-controlled system for a mobile robot. for researchers attempting to design a visualSection remote-controlled system forrobot a mobile robot. rest of The rest of the paper is organized as follows: 2 presents the mobile design and The the visual the paper is organized as follows: Section 2 presents the mobile robot design and the visual control control interface. In Section 3, the steps for the target’s 3D position estimation is provided and interface.Finally, In Section 3, the4 steps theconcluding target’s 3D comments. position estimation is provided and verified. Finally, verified. Section offersfor brief Section 4 offers brief concluding comments. 2. The Mobile Robot Design and the Visual Control Interface 2. The Mobile Robot Design and the Visual Control Interface The proposed robotic remote control system is shown in Figure 1. Wi-Fi is used as the wireless The proposed robotic remote control system is shown in Figure 1. Wi-Fi is used as the wireless communication because of its prevalence in many devices, such as personal computers, video-game communication because of its prevalence in many devices, such as personal computers, video-game consoles, and smartphones. The Arduino Yun board, based on the ATmega32u4 and the Atheros consoles, and smartphones. The Arduino Yun board, based on the ATmega32u4 and the Atheros AR9331, is employed as the mobile robot controller. The board has built-in Ethernet and Wi-Fi AR9331, is employed as the mobile robot controller. The board has built-in Ethernet and Wi-Fi support, a USB-A port, a micro-SD card slot, and a micro USB connection. The proposed mobile robot support, a USB-A port, a micro-SD card slot, and a micro USB connection. The proposed mobile operates as a mobile Wi-Fi hotspot, which is specfically for the remote controller, and achieves robot operates as a mobile Wi-Fi hotspot, which is specfically for the remote controller, and achieves smooth communication and control. The software installation or system configuration can be finished smooth communication and control. The software installation or system configuration can be finished wirelessly as the Arduino Yun runs Linino (a MIPS GNU/Linux based on OpenWRT) with the wirelessly as the Arduino Yun runs Linino (a MIPS GNU/Linux based on OpenWRT) with the Atheros Atheros AR9331 [1]. AR9331 [1].

Figure Figure1.1.The Therobotic roboticremote remotecontrol controlsystem systemarchitecture. architecture.

The TheMicrosoft MicrosoftLifeCam LifeCamHD-3000 HD-3000Webcam Webcamwith withaaUSB USBinterface interfaceisisconnected connectedto tothe themobile mobilerobot robot for real-time video transmission. Software packages, such as the USB Video Class (UVC) driver, for real-time video transmission. Software packages, such as the USB Video Class (UVC) driver,need need to be installed on the Arduino Yun. Finally, the PuTTY terminal emulator is applied, which supports to be installed on the Arduino Yun. Finally, the PuTTY terminal emulator is applied, which supports SSH (Secure Shell), a terminal protocol for a secure connection between the computer and the Yun

Sensors 2016, 16, 435

3 of 12

Sensors 2016, 16, 435 12 SSH (Secure Shell), a terminal protocol for a secure connection between the computer and the3 of Yun [12]. Sensors 2016, 16,the 435 network video taken from the webcam connected to the Arduino Yun, 3 of 12 Figure 2 shows where [12]. Figure 2 shows the network video taken from the webcam connected to the Arduino Yun, where 192.168.240 is the IP and 8080 is is the 192.168.240 is Yun the Yun IP and 8080 theport portnumber. number.

[12]. Figure 2 shows the network video taken from the webcam connected to the Arduino Yun, where 192.168.240 is the Yun IP and 8080 is the port number.

Figure 2. Network video from webcam connected to Arduino Yun.

Figure 2. Network video from webcam connected to Arduino Yun. Figure 2. Network video from webcam connected to Arduino Yun.

Figure 3 shows the completed mobile car. A total of four DC servo motors are used for the robot arm. Figure The bottom onethe (Motor A) ismobile for basecar. rotation andof top one (Motor D) isare for theused opening androbot Figure 3 shows the completed A ofthe four DC servo motors are forrobot the 3 shows completed mobile car. A total total four DC servo motors used for the closing of the claw. The middle two servo motors (Motors B and C) are for the stretch and retraction arm. arm. The bottom oneone (Motor A)A) is isfor andthe thetop topone one (Motor is for opening The bottom (Motor forbase baserotation rotation and (Motor D) D) is for the the opening and and movement of the robotic arm. two closing of claw. the claw. The middle twoservo servomotors motors (Motors (Motors BBand areare forfor thethe stretch and and retraction closing of the The middle andC)C) stretch retraction movement of robotic the robotic arm. movement of the arm.

Figure 3. The mobile robot. Figure3.3. The The mobile Figure mobilerobot. robot. A user-friendly control interface is important for manipulating the mobile robot. Figure 4 shows the visual control interface with four buttons the bottom left the directional movement A user-friendly controldesign interface is important forin manipulating thefor mobile robot. Figure 4 shows of the robot. The buttons in the bottom right control the robotic arm: the “T” and “R” buttons are 4for A user-friendly control interface is important for manipulating the mobile robot. Figure shows the visual control interface design with four buttons in the bottom left for the directional movement Motor D which controls the clamping and releasing ; the “right” and “left” buttons are for Motor A the visual control interface design with four buttons in the bottom left for the directional movement of the robot. The buttons in the bottom right control the robotic arm: the “T” and “R” buttons are for controls base rotation; and the and “forward” and; the “backward” buttons are for and Motors and C, of thewhich robot. The buttons inthe the bottom right control the“right” robotic arm: the “T” buttons Motor D which controls clamping releasing and “left” buttons are “R” for BMotor A are which balance the claw and perform the claw’s stretch and retraction movement. Moreover, the top whichDcontrols rotation; and the “forward” and “backward” buttons are forbuttons Motors are B and C, for Motor which base controls the clamping and releasing ; the “right” and “left” for Motor left button is for capturing frame, which storedand in an SD card.movement. Finally, with each click oftop all which balance the claw andvideo perform claw’sisstretch retraction Moreover, the A which controls base rotation; and thethe “forward” and “backward” buttons are for Motors B and C, the bottomisright buttons by the user, the Arduino Yun will out Finally, PWM signals to control leftbalance button forclaw capturing frame, is stored an send SD card. with each click ofthe all the which the and video perform thewhich claw’s stretchinand retraction movement. Moreover, corresponding servo motor to rotate at a fixed angle. The user can click the top right button and a the bottom right buttons by the user, the Arduino Yun will send out PWM signals to control the top left button is for capturing video frame, which isofstored in anwhich SD card. Finally, with each click drop-down menu pops out with 1°, 5°, 10°, 20°, or 50° movement is to set the fixed rotation corresponding servo motor to rotate at a fixed angle. The user can click the top right button and a

of all the bottom right buttons by the user, the Arduino Yun will send out PWM signals to control drop-down menu pops out with 1°, 5°, 10°, 20°, or 50° of movement which is to set the fixed rotation the corresponding servo motor to rotate at a fixed angle. The user can click the top right button

Sensors 2016, 16, 435

4 of 12

Sensors 2016, 16, 435 4 of 12 and a drop-down menu pops out with 1˝ , 5˝ , 10˝ , 20˝ , or 50˝ of movement which is to set the fixed rotation angle. The video clips showing how the mobile robot grabs an object have been uploaded to angle. SensorsThe 2016,video 16, 435clips showing how the mobile robot grabs an object have been uploaded to YouTube 4 of 12 YouTube forreaders’ the readers’ reference. [13][13] for the reference.

angle. The video clips showing how the mobile robot grabs an object have been uploaded to YouTube [13] for the readers’ reference.

Figure 4. The control interface design.

Figure 4. The control interface design. Figure 4. The control interface design. 3. The Target’s 3D Position Estimation

3. The Target’s 3D Position Estimation

In this section, we showEstimation how to estimate the target’s 3D position with motion stereo vision. 3. The Target’s 3D Position

the proposed flowhow of the robot’s movement and3D calculation described. this, a Firstly, InFirstly, this section, we show to estimate the target’s positioniswith motionFollowing stereo vision. In this section,iswe show how to estimate the target’s 3D position with motion stereo vision. practical estimation conducted to verify the proposed approach. the proposed flow of the robot’s movement and calculation is described. Following this, a practical Firstly, the proposed flow of the robot’s movement and calculation is described. Following this, a estimation is conducted thetoproposed practical estimationto is verify conducted verify the approach. proposed approach. 3.1. The Proposed Flow of Robot Movement and Calculation

First, the static mapping betweenand the 3D real-world coordinates and the 2D image plane is 3.1. The 3.1. Proposed Flow of Robot Movement The Proposed Flow of Robot Movement andCalculation Calculation shown in Figure 5, where

f is the focal length and the pinhole camera is assumed. There are three

First, the mapping static mapping between thereal-world 3D real-world coordinates and imageplane planeisisshown First, the static between the 3D coordinates and thethe2D2Dimage coordinate systems in Figure 5, and each has its coordinate origin. f is theand shown in Figure 5, where focalthe length and the pinholeiscamera is assumed. There are three in Figure 5, where f is the focal length pinhole camera assumed. There are three coordinate Figure and each has itsorigin. coordinate origin. systemscoordinate in Figuresystems 5, and in each has5,its coordinate

Y

Q

Y

P P O O

Q

YI (YP) YI (YP)

OI (OP)

ƒ Xƒ X

OI (OP)

XI (XP)

Z Z Image plane (X , Y ) I

XI (XP)

I

Image plane (XI , YI) Pixel plane (XP , YP) Pixel plane (XP , YP)

Figure 5. Image projection in relation to the coordinates. Figure 5. Image projection in relation to the coordinates.

Figure 5. inXrelation the coordinates. The projection between theImage targetprojection object Q ( Q , QY , QZto ) in the real world and the P point ( PX ,

PY ) inThe theprojection image canbetween be derived by simple triangulation [14], as the shown in Equation (1). is worth the target object Q ( QX , QY , Q real world and the P It point ( PX , Z ) in

The projection between the target object Q (QX , QY , QZ ) in the real world and the P point (PX , PY ) mentioning thecan P point has thebysame coordinate values[14], in direction Y for both PY ) in the that image be derived simple triangulation as shownXinand Equation (1). real It is world worth in the image can be derived by simple triangulation [14], as shown in Equation (1). It is worth  PX I in PY  PX coordinates coordinates, is, PX values and mentioningand thatimage the P plane point has the same that coordinate direction YI .and Y for both real world mentioning that the P point has the same coordinate values in direction X and Y for both real world coordinates and image plane coordinates, that is, PX  PX I and PY  PYI . (1) coordinates and image plane coordinates, that P PY “ PYI . P is, PX “ PX and Q X  Qz X and QY  QI Z Y (1) fY fX P P P P Q  Q and Q Q  X Y z QXX “ Q and QYY “ ZQ Zf (1) zf f f Equation (1) indicates the importance of Qz , the distance in Z direction. If we can obtain the coordinates of the P point in the image plane, we can get all of the coordinates of the Q point through

due to camera pixel specification , as shown in Equation (2):

(2)

X P   X I and YP   YI

where  is defined as the pixel number in 1 cm. 5 of 12 To estimate the distance of a static object from a single camera, we need to conversely move the robot to show the distance information on the image plane, or the visual interface. Figure 6 shows the flow movement order obtain motiontostereo vision target’s 3D Qzproposed . However, as itofisrobot well known, it isinhard to to obtain thethe distance an object justfor bythe a single camera. position. The user’s touchscreen is designed to have a vertical line in the middle screen. The One more transformation is also needed between the image plane coordinates and of thethe pixel plane due proposed steps are outlined as follows: to camera pixel specification , as shown in Equation (2): Step 1: The user has to control the robot to face the target, that is, the center of the target should be located on the vertical line on theXtouchscreen. YP “ the αY Iuser must click on the target and the(2) P “ αX I and Then system will check and record this point. Once the check has passed, the process will be successfully activated. where α is defined as the pixel number in 1 cm. 2: Thethe robot automatically rotates degree to thecamera, left, and we the need targetto will move to the right ToStep estimate distance of a static object from a single conversely move the on the tablet screen. robot to show the distance information on the image plane, or the visual interface. Figure 6 shows d , andvision Step 3: The robot automatically advances forward towards the target move the proposed flow of robot movement in order to obtain the distance motion stereo for will the target’s to the right on the tablet screen. The user needs to click on the target on the screen again to end the 3D position. The user’s touchscreen is designed to have a vertical line in the middle of the screen. process, and steps the target’s 3D coordinates will be estimated. The proposed are outlined as follows: Sensors 2016, 16, 435

Step1 Target

Tablet

Display

Target Mobile robot

Step2 Target

Tablet

Display

Target Mobile robot

Sensors 2016, 16, 435

6 of 12

Step3

Figure 6. Cont. Target

Tablet

Display

ω

Target Advance

Mobile robot

d

Figure6. 6. Flow Flow of of robot robot movement. Figure movement.

Figure 7 clearly depicts the robot movement viewed from above. The starting point O is the projection center of the camera in the mobile robot. In Step 2, the robot rotates  degrees to the left, and the target points Q1 and Q2 will project to the same position with respect to coordinate X. Only when the robot continues to advance towards the target in Step 3 will the target points Q1 and Q2 separate on the image plane in the X direction. Let us take target Q1 for example, where w is the distance to the middle of the new image plane. The w value contains the distance information for

Target Display

Sensors 2016, 16, 435

6 of 12 Tablet

ω

Target

Mobile

Step 1: The user has to control the robot to face the target, that is, the center of the target should be robot Advance located on the vertical line on the touchscreen. Then the user must click on the target and the system d will check and record this point. Once the check has passed, the process will be successfully activated. Step 2: The robot automatically rotates θ degree to the left, and the target will move to the right on the tablet screen. Figure 6. Flow of robottowards movement. Step 3: The robot automatically advances forward distance d, and the target will move to the right on the tablet screen. The user needs to click on the target on the screen again to end the Figure clearly depicts the robot movement viewed from above. The starting point O is the process, and 7the target’s 3D coordinates will be estimated.  degrees projection of the camera inrobot the mobile robot. viewed In Step 2, the above. robot rotates Figurecenter 7 clearly depicts the movement from The starting pointtoOthe is left, the 1 and Q 2 will projectrobot. to theIn same withrotates respect to coordinate X. Only and the target points projection center of theQcamera in the mobile Stepposition 2, the robot θ degrees to the left, and when the points robot continues advance the position target inwith Steprespect 3 will to thecoordinate target points Q1 and Q2 the target Q1 and Q2to will projecttowards to the same X. Only when separate the image plane intowards the X direction. us take target 1 for points example, is the the robot on continues to advance the targetLet in Step 3 will the Q target Q1 where and Q2 w separate distance to the middle of the new image The the distance w value on the image plane in the X direction. Letplane. us take target Q1 forcontains example, where w is information the distancefor to targets in the Z direction. the middle of the new image plane. The w value contains the distance information for targets in the Z direction.

Z

ZNEW

Q2

Q1 ω

G B f

New image (pixel) plane

PN

ß Φ

ONEW

D

d

PO θ

O

Image (pixel) plane

X

Figure 7. The top view depiction of robot movement. Figure 7. The top view depiction of robot movement.

Firstly, Firstly,the thedistance distancefrom fromstarting startingpoint pointOOto toQQ11 can can be be calculated calculated by: by: OQ1 “ OD ` DQ1 “ d cos θ ` DQ1

(3)

The distance DQ1 can be calculated by: DQ1 “ O NEW D tan φ “ d sin θ tan φ

(4)

Sensors 2016, 16, 435

7 of 12

Moreover, angle ϕ can be calculated by: φ “ π´β´p

π π ´ θq “ ´ β ` θ 2 2

(5)

Finally, we can finish the calculation by obtaining angle β through the following equation: β “ tan´1

w f

(6)

Based on the above derivation, it will be easier to obtain the 3D position of target Q1 with respect to origins O or O NEW . As shown in Figure 7, PO and P N are the projected points of Q1 in the original pixel plane and the new pixel plane, respectively. The respective coordinates (POpXP q , POpYP q ), where POpXP q – 0, and (PN pXP q , PN pYP q ) are recorded during the user’s two clicks in Step 1 and Step 3. Then, the real-world coordinate of Q1 with respect to origin O will be: Q1 pw.r.t Oq “ p0, OQ1

POpYP q fα

, OQ1 q

(7)

where the Y coordinate value is obtained by Equations (1) and (2), as shown below: Q1pYq pw.r.t Oq “ Q1pZq

POpYq f

“ OQ1

POpYI q f

“ OQ1

POpYP q fα

(8)

We can also get the real-world coordinate of Q1 with respect to origin O NEW . First, the Z direction will be changed as Z NEW direction, as shown in Figure 7. The distance of Q1 in Z NEW direction can be calculated by: O NEW G “ OG ´ d “ OQ1 cos θ ´ d (9) Then by applying Equations (1) and (2), we can get the real-world coordinate of Q1 with respect to origin O NEW as in the following equation: Q1 pw.r.t ONEW q “ pO NEW G

PN pXP q fα

, O NEW G

PN pYP q fα

, O NEW Gq

(10)

Finally, let us note that for Q1 pw.r.t Oq and Q1 pw.r.t ONEW q , their respective Y coordinate values which represent the height of the target, should be the same. So we have the following equation by Equations (7) and (10): POpYP q PN pYP q OQ1 “ O NEW G (11) fα fα 3.2. Measurement Verification To verify the proposed prototype method and the derived calculation, a tablet with a 9.7 inch display and a 960 ˆ 720 pixels camera was applied as the visual control interface. Figure 8 shows the tablet on top of a metal box which was sandwiched between two bookshelves. Using a long ruler and protractor, we moved the box to simulate the movement and rotation of the mobile robot.

Sensors 2016, 435 Sensors 2016, 16,16, 435

8 8ofof1212

tablet on top of a metal box which was sandwiched between two bookshelves. Using a long ruler and protractor,we wemoved movedthe thebox boxtotosimulate simulatethe themovement movement and and rotation protractor, rotation of of the the mobile mobilerobot. robot.

tablet on16,top Sensors 2016, 435of a metal box which was sandwiched between two bookshelves. Using a long ruler and 8 of 12

Figure 8. Measurement verification by tablet.

Figure8.8.Measurement Measurement verification Figure verificationby bytablet. tablet. First, the parameter  is about 48.7 pixel/cm according to the tablet screen size and the pixel First, the parameter pixel/cm according the tablet screen sizelength and thefpixel  is about specification (960 × 720 pixels) of the 48.7 camera. Furthermore, thetomeasurement of focal is First, the parameter α is about 48.7 pixel/cm according to the tablet screen size and the pixel specification (960 × 720 pixels) of the camera. Furthermore, thea measurement of focal length necessary for a calibrated camera system. In our measurement, room was plastered with stickersf atis

specification (960 ˆ 720 pixels) of the camera. Furthermore, the measurement of focal length f is differentfor distances on thecamera floor, with a movable pole labelled with heights. Withoutat necessary a calibrated system. In our vertical measurement, a room wasdifferent plastered with stickers necessary for a calibrated camera system. In our measurement, a room was plastered with stickers at different distances on the floor, with a movablePXvertical pole labelled with different heights. Without f , soWithout f loss distances of generality, focus on aQmovable in Equation (1) to estimate focal length different on we thewill floor, with pole labelled with different heights. X  Qz vertical PXf loss of generality, we will focus on Q X  Qz PX in Equation (1) to estimate focal length f , so f loss of generality, we will focus P on Q X “ Qz f in Equation (1) to estimate focal length f, so f can f X can be calculated by f  Qz . In Figure 9, the vertical pole was put at distances ranging from PX P QX X be calculated by f “by Qz f .Q In Figure 9, the vertical pole was put at distances ranging from near to can be calculated Q X z Q . In Figure 9, the vertical pole was put at distances ranging from Q QX was near to far away, with ranging from tobut 250 Q cm, but kept at 36 cm. The distance X z far away, with Qz ranging from 100 cm to 100 250cm cm, at 36 cm. The distance between X was kept Qzeach Q to double far the away, with ranging 100 cm to cm, but was kept at 36 cm. The distance P P the near white arrow in picture indicates P250 (pixel) or P (cm). All of the data for the between white double arrow infrom each picture indicates (pixel) or (cm). All of the data for four XX XP XP X different cases is shown in Table 1. average f was calculated asor 24.03, which isofadopted in the PX P f(pixel) PX (cm). between thedifferent white double arrow in The each picture indicates the data for the four cases is shown in Table 1. The average was calculated asAll 24.03, which is subsequent calculations. adopted in the subsequent calculations. the four different cases is shown in Table 1. The average

f was calculated as 24.03, which is

adopted in the subsequent calculations.

Sensors 2016, 16, 435

(a)

(a)

PX

PX

= 8.54 cm Figure 9. Cont.

= 8.54 cm

(b)

PX

= 5.75 cm

(b)

PX

= 5.75 cm

9 of 12

Figure 9. Cont.

(c)

PX

= 4.35 cm

(d)

PX

= 3.49 cm

PX (c)= P5.75 f measurement Figure 9. Focal length ( Qcm). 36 Pcm). 8.54 (b) cm; Figure 9. Focal length f measurement (QX = 36 8.54P cm; PXcm; = 5.75 4.35 cm; X = (a) X = cm; X = (a) X =(b) (d) PX(c)= P 3.49 =cm. 4.35 cm; (d) P = 3.49 cm. X

X

Table 1. Results of focal length

PX P

(pixel) 416

PX

(cm)

8.54

f

measurement ( QX = 36 cm).

QZ

(cm)

100

f (cm) 23.72

Figure 9. Focal length (c)

PX

= 4.35 cm; (d)

PX

measurement ( QX = 36 cm). (a)

PX

(pixel)

PX

= 8.54 cm; (b)

f

= 5.75 cm;

measurement ( QX = 36 cm).

QZ

(cm)

(cm)

Table 1. Results of focal length f measurement (QX = 36 cm).

416 280 212 170

PX

= 3.49 cm.

Table 1. Results of focal length

Sensors 2016, 16, 435

PX P

f

8.54 PXP5.75 (pixel) 4.35 416 3.49 280

PX (cm)

100 150 QZ (cm) 200 100 250 150

f (cm)

9 of 12

f (cm) 23.72 23.96 24.17 24.27

8.54 23.72 5.75 23.96 212 4.35 200 24.17 170 3.49 250 24.27 Next, we will verify the proposed flow for estimating the 3D position of a target object. The



is set to 12° and d is 20 cm. For simplicity, we will calculate OQ1 in Equation (3) for the following Next, we will verify the proposed flow for estimating the 3D position of a target object. The θ verification. For the first step in the proposed process, we put the vertical pole in the middle of the is set to 12˝ and d is 20 cm. For simplicity, we will calculate OQ1 in Equation (3) for the following screen. Then in the Step 2, the robot was rotated 12° to the left and the vertical pole moved to the verification. For the first step in the proposed process, we put the vertical pole in the middle of the right on the screen. Finally, as the robot advanced 20 cm in Step 3, the vertical pole moved to the right screen. Then in the Step 2, the robot was rotated 12˝ to the left and the vertical pole moved to the right again. Figure 10 shows two extreme cases for the target distance measurement after Step 3 was on the screen. Finally, as the robot advanced 20 cm in Step 3, the vertical pole moved to the right again. finished, equals 50 cm and 250target cm, which corresponds to the nearest and3 farthest cases, Figure 10where shows OQ two1 extreme cases for the distance measurement after Step was finished, where OQ1 equals 50 cm and 250 cm, which to the and farthest cases, PN(respectively. respectively. The distance between the whitecorresponds double arrows in nearest each picture indicates X P ) (pixel) The distance between the white double arrows in each picture indicates PN pXP q (pixel) or w (cm). or (cm).

w

(a)

OQ1 = 50 cm

(b)

OQ1 = 250 cm

Figure Measurement of distance to the target. OQ1 ==50 50 cm; cm; (b) cm.cm. OQ OQ 1 1= 250 Figure 10. 10. Measurement of distance to the target. (a) (a) (b) OQ = 250 1

Using the derived equations from the last subsection, the step by step procedure of OQ1 calculation Using the derived equations from the last subsection, the step by step procedure of OQ1 for the target’s 3D estimation algorithm can be summarized as: calculation for the target’s 3D estimation algorithm can be summarized as: ‚ Given constant values: θ, d, α, f. ‚ Get the coordinates (POpXP q , POpYP q ) and (PN pXP q , PN pYP q ) at the start (Step 1) and end (Step 3) of PN pXP q the proposed approach. The variable w equals to . α ‚ Calculate β by Equation (6). ‚ Calculate ϕ by Equation (5). ‚ Calculate DQ1 by Equation (4). ‚ Calculate OQ1 by Equation (3). ‚ Estimation for the 3D coordinates of target Q1 with respect to origins O or O NEW can be accomplished by Equations (7) and (10). Table 2 shows the input PNpXP q and the resulting parameters and estimated OQ1 for the two cases in Figure 10. It seems that the estimation result meets the expectation that the farther away the target is, the larger the error will be, which may be true for any 2D image method used in 3D distance estimation.

Sensors 2016, 16, 435

10 of 12

Table 2. Estimation results of distance to the target.

Case 1 Case 2

PNpXP q (Pixel)

w (cm)

β (In Degree)

ϕ (In Degree)

True OQ1 (cm)

Estimated OQ1 (cm)

414 270

8.50 5.54

19.48 12.98

82.52 89.02

50 250

51.23 262.05

Let us review Figures 6 and 7 again, which show the whole process and mathematical derivation of the target’s 3D position estimation. To make the process work, “the robot automatically rotates θ degree to the left” in Step 2 is an important step. However, the key point in Step 2 is that the projection center of the camera, the O point in Figure 7, must be kept at the same point after the rotation of the mobile robot. As mentioned earlier, we can check this by observing target points at different distances, like Q1 and Q2 in Figure 7, and these should project to the points on the screen with the same X coordinate value. But in practice, it may be hard to drive a mobile robot to achieve this. So, in practice, Step 2 and Step 3 can be combined to first obtain the final correct position of the mobile robot by manual operation. After the final ideal position of the robot is obtained, we can then begin designing a method to make the mobile robot reach the correct position automatically. To implement the automatic process for target’s 3D position estimation, in addition to the above points for Step 2 and Step 3, we should also reconsider the hardware design of the mobile robot. For example, to improve the accuracy, the DC motor which controls the wheels may need to be replaced by a stepping motor. Furthermore, the odometer data often includes some errors due to tire slips, so high friction tires may be necessary. A remote-controlled mobile robot with video transmission is practical and effective when the operator is unable to see the environment or visibility is poor, but when the mobile robot is fitted with a robotic arm, it could be difficult and time-consuming for an inexperienced user to manipulate the robotic arm to grasp an object. With the proposed target’s 3D position estimation, the user can activate the automatic process when approaching the target, then the mobile robot and robotic arm may reach the appropriate position by inverse dynamics in a short time automatically. Therefore, it has functional applications for robotic grasping tasks, such as obstacle removal or bomb disposal. For the proposed Wi-Fi mobile robot design in Section 2, it appears that communication is smooth when using the video transmission and control commands. However, when the target’s 3D position estimation in this section is added, the computational consumption may cause a time delay between the click on the touchscreen and the motor reaction. Fortunately, the proposed three steps for the target’s 3D position estimation is activated by the user, and then executed by the system in an automatic way. Thus, the interactive design can be incorporated to reduce the effect of the time delay. Finally, the pitch angle of the camera will affect the positioning performance; thus, the camera and the robotic arm should be reset to the standard position once the target’s 3D position estimation is activated to avoid any error caused by the pitch angle of the camera. 4. Conclusions In this paper, a visual control interface for a mobile robot was proposed, and a simple prototype method for estimating the 3D position of a target was derived and verified. Wi-Fi technology was applied to provide real-time video transmission, which enables the robot to operate in different environments or areas where visibility is poor, limited, or the object is out of sight. It was found that, in addition to the basic movements of the robot, more control actions are needed for the manipulation of the robotic arm and a user-friendly interface is necessary for human operators. In the proposed visual control interface, there are 12 buttons in all for controlling two DC motors and four DC servo motors. Among these buttons, one special button controls a drop-down menu which allows the user to select the degree of movement which allows for a more flexible way to control the servo motor.

Sensors 2016, 16, 435

11 of 12

The authors have uploaded the operation videos to YouTube for the readers’ reference. The videos show how a skilled user operates the mobile robot to grasp an object with a three-axis robotic arm. However, this will not always be an easy task for an amateur or in an emergency situation. Thus, it will be helpful if we can estimate the 3D position of the desired target, then activate the corresponding automatic process to finish the job by robotic arm inverse dynamics. Although stereo cameras and range sensors can be applied in 3D position reconstruction, we have shown that a simple approach for the target’s 3D estimation with a single, ordinary camera is still a worthwhile research goal. Instead of a complicated theory, this paper proposes a prototype method for estimating the target’s 3D position by simple geometry and triangulation. The proposed steps have been clearly verified and, in future, we hope to make the robot motion and 3D position estimation more accurate. Acknowledgments: The authors would like to express the sincere thanks to the reviewers for their invaluable comments and suggestions. This work is supported by the Ministry of Science and Technology, Taiwan, under Grant nos. MOST 104-2221-E-218-039. Author Contributions: Juing-Shian Chiou and Chun-Tang Chao developed the methodology and drafted the manuscript. Moreover, Chi-Jo Wang derived the 3D formulas and Ming-Hsuan Chung implemented the hardware experiments. The authors approved the final manuscript. Conflicts of Interest: The authors declare no conflict of interest.

References 1. 2. 3. 4.

5.

6.

7. 8.

9.

10. 11.

12.

Arduino Yun. Available online: https://www.arduino.cc/en/Main/ArduinoBoardYun (accessed on 17 January 2016). Palma, D.; Agudo, J.E.; Sánchez, H.; Macías, M.M. An Internet of Things example: Classrooms access control over near field communication. Sensors 2014, 14, 6998–7012. [CrossRef] [PubMed] Chao, C.T.; Wang, C.W.; Chiou, J.S.; Wang, C.J. An arduino-based resonant cradle design with infant cries recognition. Sensors 2015, 15, 18934–18949. [CrossRef] [PubMed] Chen, Y.; Han, F.; Yang, Y.H.; Ma, H.; Han, Y.; Jiang, C.; Lai, H.Q.; Claffey, D.; Safar, Z.; Liu, K.J.R. Time-reversal wireless paradigm for green Internet of Things: An overview. IEEE Internet Things J. 2014, 1, 81–98. [CrossRef] Sabattini, L.; Levratti, A.; Venturi, F.; Amplo, E.; Fantuzzi, C.; Secchi, C. Experimental comparison of 3D vision sensors for mobile robot localization for industrial application: Stereo-camera and RGB-D sensor. In Proceedings of the 2012 IEEE International Conference on Control Automation Robotics & Vision (ICARCV), Guangzhou, China, 5–7 December 2012; pp. 823–828. Cai, X.; Dean-Leon, E.; Somani, N.; Knoll, A. 6D image-based visual serving for robot manipulators with uncalibrated stereo cameras. In Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Chicago, TX, USA, 14–18 September 2014; pp. 736–742. Hu, X.; Hu, Y.; Xu, B. Generalized Kalman filter tracking with multiplicative measurement noise in a wireless sensor network. IET Signal Process. 2014, 8, 467–474. [CrossRef] Wen, Z.; Cai, Z. Mean shift algorithm and its application in tracking of objects. In Proceedings of the 2006 International Conference on Machine Learning and Cybernetics, Dalian, China, 13–16 August 2006; pp. 4024–4028. Jang, W.; Oh, S.; Kim, G.A. hardware implementation of pyramidal KLT feature tracker for driving assistance systems. In Proceedings of the 2009 IEEE International Conference on Intelligent Transportation Systems (ITSC '09), St. Louis, MO, USA, 4–7 October 2009; pp. 1–6. Karungaru, S.; Ishitani, A.; Shiraishi, T.; Fukumi, M.A. simple interface for mobile robot equipped with single camera using motion stereo vision. Int. J. Mach. Learn. Comput. 2012, 2, 633–639. [CrossRef] Sudin, M.N.; Nasrudin, M.F.; Abdullah, S.N.H.S. Humanoid localization in a robot soccer competition using a single camera. In Proceedings of the 2014 IEEE Tenth International Colloquium on Signal Processing & its Applications (CSPA), Kuala Lumpur, Malaysia, 7–9 March 2014; pp. 77–81. Using the Command Line for Communication with SSH and cURL. Available online: https://www. arduino.cc/en/Tutorial/LinuxCLI (accessed on 17 January 2016).

Sensors 2016, 16, 435

13. 14.

12 of 12

A Wi-Fi Mobile Robot with Real-Time Video Transmission. Available online: https://youtu.be/ 8pajGYlh_UQ (accessed on 17 January 2016). Szeliski, R. Computer Vision: Algorithms and Applications; Springer: New York, NY, USA, 2011. © 2016 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons by Attribution (CC-BY) license (http://creativecommons.org/licenses/by/4.0/).