research article indoor slam using laser and camera with...

Research ArticleIndoor SLAM Using Laser and Camera with Closed-LoopController for NAO Humanoid Robot

Shuhuan Wen1 Kamal Mohammed Othman2 Ahmad B Rad2

Yixuan Zhang2 and Yongsheng Zhao3

1 Key Lab of Industrial Computer Control Engineering of Hebei Province Yanshan University Qinhuangdao 066004 China2 School of Engineering Sciences Simon Fraser University No 250-13450 102 Avenue Surrey BC Canada V3T 0A33 Parallel Robot and Mechatronic System Laboratory of Hebei Province and Key Laboratory of Advanced Forging ampStamping Technology and Science of Ministry of National Education Yanshan University Qinhuangdao 066004 China

Correspondence should be addressed to Ahmad B Rad aradsfuca

Received 4 May 2014 Revised 24 June 2014 Accepted 25 June 2014 Published 10 July 2014

Academic Editor Shen Yin

Copyright copy 2014 Shuhuan Wen et al This is an open access article distributed under the Creative Commons Attribution Licensewhich permits unrestricted use distribution and reproduction in any medium provided the original work is properly cited

We present a SLAM with closed-loop controller method for navigation of NAO humanoid robot from Aldebaran The method isbased on the integration of laser and vision system The camera is used to recognize the landmarks whereas the laser provides theinformation for simultaneous localization and mapping (SLAM ) K-means clustering method is implemented to extract data fromdifferent objects In addition the robot avoids the obstacles by the avoidance functionThe closed-loop controller reduces the errorbetween the real position and estimated position Finally simulation and experiments show that the proposed method is efficientand reliable for navigation in indoor environments

1 Introduction

Robot has been used in many areas such as industry process[1ndash6] and autonomous navigation Autonomous navigationin an unknown environment is regarded as a key attribute ofa service robot and has received considerable attention in thelast two decades The focus of this paper is the indoor envi-ronments such as homes offices and hospitals It is importantthat the service robots can roam around in order to assisthumans or to perform various tasks in such surroundingsFor example the robots need to detect and avoid obstaclesthat they might encounter in the environment

Precise position estimation is a necessary prerequisitefor a reliable navigation In this case simultaneous local-ization and mapping (SLAM) approach is employed tomake a robot truly autonomous without the need for anya priori knowledge of location SLAM algorithm [7 8] canbe recognized and localized in an unknown environmentSome artificial intelligence methods are used in the SLAMof the mobile robots such as reinforcement learning [9]which we will study for NAO humanoid robot in the future

SLAM algorithm is the essential process of building a mapof the environment while simultaneously determining itslocation within this map SLAM has also been implementedin a number of different domains from indoor to outdoorunderwater and airborne systems However the effectivenessof various SLAMmethods greatly depends on using differentexteroceptive sensors (sonar laser camera odometer etc)Some methods are based on vision or laser [10 11] Cameraand laser have different features respectively Cameras canprovide the 3D sensing of the environment and obstacleavoidance but its accuracy of localization is generally inferiorto laser scanner Compared to other sensors laser providesmore accurate range and bearing measurements Howeverlaser cannot provide the image of the environment Inparticular laser of the robot cannot recognize the objectwhen it is faced with landmarks or obstacles Therefore thecombination of camera and laser can achieve very good andreliable navigation for indoor SLAM problems There aresome studies reported in the literature whereby the laserand the stereo camera are used together for accomplishingindoor navigation tasks In Labayrade et alrsquos paper [12] the

Hindawi Publishing CorporationAbstract and Applied AnalysisVolume 2014 Article ID 513175 8 pageshttpdxdoiorg1011552014513175

2 Abstract and Applied Analysis

two sensors were used together to eliminate false positions inobject detection In Iocchi and Pellegrinirsquos paper [13] the 2Dlaser scanner and the stereo camerawere used for building the3D environment map In Lin et alrsquos paper [14] the proposedapproach combined SLAM using a 2D laser scanner anddense 3D depth information from a stereo camera and thiswork solved effectively the obstacles at different heights intothe 2D map The objective of this work is to effectively finishaccurate position based on SLAM and avoid the obstacle ofthe environment The proposed approach combines SLAMusing a laser and stereo cameras Using the cameras it canrecognize landmarks and provide obstacle information inthe environment Laser can provide high accuracy of bothrange and bearingmeasurements K-meansmethod is used toextract data from different objects Robots with the proposedmethod can have rich environment information for betternavigation and obstacle avoidance The proposed approachis implemented on an NAO humanoid robot

The rest of the paper is organized as follows In Section 2the background knowledge of laser based SLAMwith closed-loop controller the NAO platform that we have used andthe choosing method of the landmarks by using cameras areintroduced In Section 3 we briefly describe the task scenarioand K-means clustering method of extraction data fromLaser The method of extraction data from different objectsby using laser and avoidance obstacles is also described Theexperiment results are shown in Section 4 Finally Section 5gives the conclusion and discusses future work

2 Laser and Camera Based SLAM withClosed-Loop Controller

21 EKF-SLAM There are two key computational solutionsto the SLAM problem the extended Kalman filter (EKF-SLAM) and Rao-Blackwellized particle filters (FastSLAM)EKF-SLAM was presented by Smith and Cheeseman in1987 and has been used extensively [15] EKF-SLAM isvery well known to navigation problems The main stepsof SLAM include robot motion prediction new landmarksinitialization and known landmarks correction

The observation model is [7]

119910 = ℎ (119909) + V (1)

where 119909 is robot state vector and landmark states ℎ(sdot) is theobservation function and V is the measurement noise

The covariance matrix is

119875 = 1198651198751198651015840+ 119876 (2)

where 119865 is Jacobian matrix 119876 is the Gaussian noiseThe EKF correction step is written as

119911 = 119910 minus ℎ (1199091015840)

119885 = 119867119875119909119879+ 119877

(3)

where 119911 is the innovationrsquos mean 1199091015840 is the observationposition of the robot119867 is Jacobianmatrix119877 is the covariancematrix of the measurement noise

Figure 1 Aldebaran-robotics NAO humanoid

Kalman gain119870 is

119870 = 119875119867119879119885 (4)

The update position of the robot is

1199091015840= 1199091015840+ 119870119911 (5)

The correction covariance matrix is

119875 = 119875 minus 119870119885119870119879 (6)

The measurement data is given by the laser include thedistance angle between the robot and landmarks and theposition of the landmarks The extraction data method fromlaser is introduced in Section 3

22 NAO Humanoid Robot The platform selected for myresearch project is humanoid robot NAO a commonly usedhumanoid platform for education environment producedby the French company Aldebaran Robotics NAO is areasonably priced advanced humanoid robot with attractivefeatures that make it an ideal platform for research inhumanoid robots It is designed based on an open archi-tectural philosophy and can be programmed with severaldifferent programming environments NAO is a medium-sized humanoid robot developed mainly for universities andlaboratories for research and education purposes It replacedthe Sony AIBO dogs in the RoboCup Standard PlatformLeague (SPL) in 2008 As an autonomous humanoid robotNAO is capable to move in a biped way sense its closeenvironment communicate with human and think by on-board processor [16ndash21] NAO is a new biped robot recentlydeveloped by French company Aldebaran-Robotics Figure 1shows the overall appearance of the robot in our experimentNAO is 057meters high andweighs about 45 kilogramNAOwith laser head is an option provided by Aldebaran roboticsfor users intending to study specific navigation problems suchas SLAM

NAO is very light compared to other existing robotsof the same height because its BMI is about 135 (kgm2)NAO has a total of 25 degrees of freedom 11 degrees offreedom (DOF) for its lower parts and another 14 DOFfor its upper parts NAO robot is demonstrated generallyby its hardware mechanical architecture and software The

Abstract and Applied Analysis 3

Table 1 Main characteristics of the NAO URG-04LX laser

Function DataDetection range 002sim4mScan angle 240∘

Scan time 100msecscan (100Hz)Resolution 1mmInterface USB 20 RS232

hardware section mainly includes several sensors as they arecritical for the robot to explore the surrounding environmentIn the software the structure of NAOqi is explained by thethree components NAOqi OS NAOqi Library and Devicecontrol Manager In addition the dedicated NAO softwareChoregraphe Monitor and Webots are included

There is one laser scanner on the robot NAO headBesides the camera the laser sensor is another essentialdevice for my research The SLAM algorithm requires thelaser to obtain landmark location information in terms ofbearing and distance to perform a full SLAM process Thetype of the laser used in our experiment is URG-04LX Thefunction of the laser is provided in Table 1

The robot has a CPU in its v40 head of our experiment asthe main processing unit itrsquos a GEODE 500MHz board with512M of flash memory andWi-Fi connection for connectionto the development remote computer [22] NAO also has auser friendlyGUI so it is easy to programNAOusingC C++Phython or Urbi Python 27 is used to program in this paper

23 Closed-Loop Controller NAO deviates from a set targetas there is no closed-loop control to compensate the noise ofthe sensors This results in an inaccurate position of NAOFigure 2 shows the experiment result of the real positionand estimated position of the NAO when the control 119880 is afixed value The error is large between the real position andthe actual robot position Therefore we designed a simplePI controller in order to reduce the error The closed-loopprocess is only based on the return error of the real positionand the estimated position ofNAOTheoutline of this closed-loop is presented in Figure 3 where 119909

119877is the real position

119909119890119904is the EKF-estimated position 120576 is the threshold defined

by user and 119896119901is the proportional value The steps of closed-

loop controller are followedClosed-loop controller steps are as follows

(i) define an initial controller value 119880 = Constant (ie119880 = 01)

(ii) if the error 119890 = 119909119877minus119909119890119904gt 120576 then119880 = 119896

119901(119890+119879119879

119894sum119890)

else 119880 = Constant (ie 119880 = 01)

The real position and EKF-estimated position of the NAOwith closed-loop controller are shown in Figure 4

The error between estimated and reference robot positioncan be calculated which is in significant amount duringthe experiment Accordingly a closed-loop PI controller isimplemented to minimize this error such that the deviationthat robot travels can be reduced In addition to verify theperformance ofmotion controller the result has been verified

20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20

Real positionEstimated position

x (m)

y(m

)

Figure 2 The experiment motion results of robot without closed-loop controller

e

PI controllerU

xR

xes

+

summinus

Figure 3 The closed-loop controller

20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)

Figure 4 The experiment motion results of robot with closed-loopcontroller


with observation during the experiment It has shown that themotion controller has succeeded in keeping NAO robot withsmall position error

24 Choosing Landmarks by Using Cameras In order todistinguish the landmarks from the obstacles in the envi-ronment we used the NAO embedded vision system as therobot is not able to decide if an object is a landmark or anobstacle Firstly the robot chooses the objects as landmarksand recognizes them Then the robot reads the laser datawhen it sees the landmarks The recognizing object functionis used in this paper The steps of recognizing objects arefollowed

Recognizing objects steps are as follows

(i) take a picture for the recognized objects by thecameras

(ii) draw the contour of the objects(iii) save the message of the recognized objects to vision

recognition database(iv) create a Python module called back of picture recog-

nition(v) call laser when robot sees the objects

The recognized objects are shown in Figure 5

3 Task Scenario

31 Task Scenario We test our navigation approach in anindoor environment There are two landmarks in the envi-ronment (see Figure 6)

In the scenario the NAO is supposed to walkautonomously from its current location to a destination Therobot needs to recognize the landmarks and it can avoid theobstacle when it walks An outline of implement the task isshown in Figure 7

In order to finish the navigation indoors environment werequire several components including

(i) recognition of objects by using camera that judge theobjects that are landmarks or obstacles

(ii) navigation in an unknown environment that callsSLAM when meeting landmarks

(iii) a lasermodule that provides the robotrsquos positionwhilewalking

(iv) an avoidance module that calls the function whenmeeting obstacle

32 The K-Means Clustering Method of Extraction Data fromLaser It is very important to extract different objects datafrom laser The data is provided in a two-dimensional array684 by 4 in our experiment Each index includes length(distance between laser and objects) angle (computed valuebased on the index andURGdevice configuration) and119909 and119910 (objects coordinates in Cartesian space)

The k-means algorithm is an effective method of extrac-tion laser data The term ldquok-meansrdquo was first used by

(a) The picture of the landmark

(b) The contour of the landmark

Figure 5 The process of the recognized landmarks

Figure 6 The scenario of the experiment

MacQueen in 1967 [23] The steps are followed The k-meansalgorithm takes as input the number of clusters and a set ofobservation vectors to cluster It returns a set of centroids onefor each of the clusters An observation vector is classifiedwith the cluster number or centroid index of the closestcentroid to it


LandmarkStartendposition

Camerarecognize

objectYes

No

Readlaser data

Callavoidance

obstacle function

CallSLAM withclosed-loopcontrollerfunction

Robotmove

Lastlandmark

Yes

No

Figure 7 The overall flow chart of the navigation

Table 2 Main python code for using obstacle avoidance function

Function Python codeCalling ALNavigation module navigationProxy=ALProxy(ldquoALNavigationrdquo robotIP PORT)Moving NAO amp use its sonars navigationProxymoveTo(xytheta)

Step 1 Assign each observation to the cluster whose mean isthe closest to it

119878119894= 10038171003817100381710038171003817119909119901minus 119898119894

10038171003817100381710038171003817le10038171003817100381710038171003817119909119901minus 119898119895

10038171003817100381710038171003817 forall1 le 119895 le 119896 (7)

where119909119901is a set of observations 119909

1sdot sdot sdot 119909119899 assigned to exactly

one 119878 and119898119894is an initial set of 119896means 119898

1sdot sdot sdot 119898119896

Step 2 Calculate the new means to be the centroids of theobservations in the new clusters

119898119894=110038161003816100381610038161198781198941003816100381610038161003816

sum

119909119895isin119878119894

119909119895 (8)

The algorithmhas convergedwhen the assignments no longerchange There are two landmarks in our experiment

33 Avoidance Obstacles In order to prevent the robot toknock down other objects we use avoidance in our experi-mentThe avoidance function is navigationProxymoveTo (sdot)The key of addressing this issue is calling the ALNavigationmodule fromNAOqi as shown in Table 2Thismodule assiststhe robot to use its sonars while it is in motion By usingsonars the robot is able to stop moving if there are anyobstacles located in the security area of sonarsThe avoidancesteps are followed

Avoidance obstacles steps are as follows

(i) call navigationProxymoveTo (sdot) function

(ii) if there exit obstacles the robot stops and turns to acertain angle Assume the security distance is 03mbetween the robot and the obstacles

else walks forward

Table 3 The value of the experiment parameters

Parameters ValuelasersetDetectingLength 002sim15mlasersetOpeningAngle minus90∘ sim90∘

119896119901

08119879119894

55119880 01

4 Simulation and Experiment Results

The proposed method has been evaluated in the autonomousand intelligent systems laboratory (AISL) There are twolandmarks in our experiment In order to test our methodfirstly we did the simulation on the computerThe simulationresult is shown in Figure 8 and the result is good Then wedid the experiment in the labThe experiment parameters areshown in Table 3

Figure 9 is the experimental result of SLAM withoutclosed-loop controller The two triangles are the initial posi-tion of the robotThe two stars are the landmarksThe circleson the red points are the covariance of the robot The circlesbecome smaller when the robot is near the landmark Thecontrol input is 01 and the error between the real positionand estimated position is bigger and bigger because it is anopen loop

In Figure 10 the error between the real position andestimated position becomes smaller and smaller because PIcontroller is added An obstacle is added based on Figure 10The experiment result is shown in Figures 11 and 12 Theyellow circle is the obstacle The red point is the estimatedposition of the NAO robot The motion of the robot ischanged because there exits obstacle The experiment showsthat the robot can avoid the obstacles successfully


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)

Figure 8The simulationmotion result of Robot SLAMwith closed-loop controller

20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)

Figure 9 The experiment motion result of Robot SLAM withoutclosed-loop controller

5 Conclusion and Future Work

The main contribution of this paper was the full real timeimplementation of EKF-SLAM on the NAO humanoid robotby combining its camera with laser EKF-SLAM algorithmwas realized in Python code and studied in simulationand experimental implementation The simulation resultvalidated the effort of EKF-SLAM landmark observationin terms of reducing the uncertainty of robot motion Thecamera is used to recognize the landmarks and then thelaser gave the distance and the angle between the robot and

20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)

Figure 10 The experiment motion result of Robot SLAM withclosed-loop controller

20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)

Figure 11 The experiment motion result of Robot SLAM withclosed-loop controller and avoidance obstacle

landmarks The robot can avoid the obstacle by using nav-igationProxymoveTo (sdot) function A closed-loop PI motioncontroller was used to improve trajectory control The exper-iment was the realization of simulation in two landmarkscases and the NAO robot in the experiment successfullyaccomplished EKF-SLAM in a realistic exploration task withtwo landmarks and a fixed obstacle Our future works includedesigning an avoidance algorithm to choose the best pathwhich can avoid the obstacles rather than use the existingavoidance function The controller is also to be improvedin order to obtain better accurate position We used one


02

01

03

020 025 030

00

015

minus01


x (m)

y(m

)

Figure 12The enlargement result based on Figure 11 in order to seeclearly

landmark at one time in our paper but more landmarks areused at one time in the future work

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

The work was partly supported by the Natural Science Foun-dation of Hebei Province of China (Project no F2014203095)the Young Teacher of Yanshan University (Project no13LGA007) the National Natural Science Foundation ofChina (Project no 51275439) and the Major State BasicResearch Development Program of China 973 Program(Project no 2013CB733000)

References

[1] S Yin S Ding X Xie and H Luo ldquoA review on basic data-driven approaches for industrial process monitoringrdquo IEEETransactions on Industrial Electronics vol 61 no 11 pp 6418ndash6428 2014

[2] S Yin X Li H Gao and O Kaynak ldquoData-based techniquesfocused on modern industry an overviewrdquo IEEE Transactionson Industrial Electronics 2014

[3] S Yin H Gao and O Kaynak ldquoData driven control andprocess monitoring for industrial applicationsmdashpart IIrdquo IEEETransactions on Industrial Electronics 2014

[4] S Yin X Gao H R Karimi and X P Zhu ldquoStudy on supportvector machine-based fault detection in Tennessee eastmanprocessrdquo Abstract and Applied Analysis vol 2014 Article ID836895 8 pages 2014

[5] S Yin X P Zhu and H R Karimi ldquoQuality evaluation basedon multivariate statistical methodsrdquo Mathematical Problems inEngineering vol 2013 Article ID 639652 10 pages 2013

[6] X Xie S Yin H Gao and O Kaynak ldquoAsymptotic stabilityand stabilisation of uncertain delta operator systems with time-varying delaysrdquo IET Control Theory and Applications vol 7 no8 pp 1071ndash1078 2013

[7] H Durrant-Whyte and T Bailey ldquoSimultaneous localizationand mapping (slam)mdashpart I the essential algorithmsrdquo IEEERobotics and Automation Magazine vol 13 no 2 pp 99ndash1082006

[8] T Bailey and H Durrant-Whyte ldquoSimultaneous localizationand mapping (SLAM) part IIrdquo IEEE Robotics and AutomationMagazine vol 13 no 3 pp 108ndash117 2006

[9] W D Smart and L P Kaelbling ldquoEffective reinforcementlearning for mobile robotsrdquo in Proceedings of the EEERSJInternational Conference on Robotics and Automation (ICRArsquo02) pp 3404ndash3410 IEEE Press Washington DC USA May2002

[10] A J Davison I D Reid N D Molton and O StasseldquoMonoSLAM real-time single camera SLAMrdquo IEEE Transac-tions on Pattern Analysis andMachine Intelligence vol 29 no 6pp 1052ndash1067 2007

[11] D Fox D Hahnel W Burgard and S Thrun ldquoAn efficientfastslam algorithm for generating maps of large-scale cyclicenvironments from raw laser range measurementsrdquo in Pro-ceedings of the IEEERSJ International Conference on IntelligentRobots and Systems (IROS rsquo03) pp 206ndash211 IEEE Press LasVegas Nev USA October 2003

[12] R Labayrade C Royere D Gruyer and D Aubert ldquoCoopera-tive fusion formulti-obstacles detectionwith use of stereovisionand laser scannerrdquo Autonomous Robots vol 19 no 2 pp 117ndash140 2005

[13] L Iocchi and S Pellegrini ldquoBuilding 3D maps with semanticelements integrating 2D laser stereo vision and INS on amobile robotrdquo in Proceedings of the 2nd International Society forPhotogrammetry and Remote Sensing International Workshop3D-ARCH (ISPRS rsquo07) pp 1ndash8 IEEEPress Zurich Switzerland2007

[14] K Lin C Chang and A a Dopfer ldquoMapping and localizationin 3D environments using a 2D Laser scanner and a stereocamerardquo JISE Journal of Information Science and Engineeringvol 28 no 1 pp 131ndash144 2012

[15] R C Smith and P Cheeseman ldquoOn the representation and esti-mation of spatial uncertaintyrdquo International Journal of RoboticsResearch vol 5 no 4 pp 56ndash68 1986

[16] D Gouailier V Hugel P Blazevic et al ldquoMechatronic designof nao humanoidrdquo in Proceedings of the IEEE InternationalConference on Robotics and Automation (ICRA rsquo09) pp 769ndash774 IEEE Press Kobe Japan May 2009

[17] S Shamsuddin L I Ismail H Yussof et al ldquoHumanoidrobot NAO review of control and motion explorationrdquo inProceedings of the IEEE International Conference on ControlSystem Computing and Engineering (ICCSCE rsquo11) pp 511ndash516IEEE Press Penang Malaysia November 2011

[18] D Tlalolini C Chevallereau and Y Aoustin ldquoOptimal refer-encemotions with rotation of the feet for a bipedrdquo in Proceedingof the ASME International Design Engineering Technical Confer-ences and Computers and Information in Engineering Conference(IDETCCIE 08) pp 1027ndash1036 IEEE Press New York NYUSA August 2008


[19] K Nishiwaki S Kagami Y Kuniyoshi M Inaba and HInoue ldquoToe joints that enhance bipedal and fullbody motionof humanoid robotsrdquo in Proceedings of the IEEE InternationalConference on Robotics adn Automation (ICRA rsquo02) pp 3105ndash3110 IEEE Press Washington DC USA May 2002

[20] Y Ogura K Shimomura H Kondo et al ldquoHuman-like walkingwith knee stretched heel-contact and toe-off motion by ahumanoid robotrdquo in Proceedings of the IEEERSJ InternationalConference on Intelligent Robots and Systems (IROS rsquo06) pp3976ndash3981 October 2006

[21] S Lohmeier T Buschmann H Ulbrich and F Pfeiffer ldquoMod-ular joint design for performance enhanced humanoid robotLOLArdquo in Proceedings of the IEEE International Conference onRobotics and Automation (ICRA rsquo06) pp 88ndash93 IEEE PressOrlando Fla USA May 2006

[22] O Mohareri and A B Rad ldquoAutonomous humanoid robotnavigation using augmented reality techniquerdquo in Proceedingsof the IEEE International Conference on Mechatronics (ICM rsquo11)pp 463ndash468 IEEE Press Istanbul Turkey April 2011

[23] J B MacQueen ldquoSome methods for classification and analysisof multivariate observationsrdquo in Proceedings of the 5th BerkeleySymposium on Mathematical Statistics and Probability (MSAPrsquo67) pp 281ndash297 IEEE Press New York NY USA 1967

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of


Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of


Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of


Mathematical PhysicsAdvances in

Complex AnalysisJournal of


OptimizationJournal of


CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of


Operations ResearchAdvances in

Journal of


Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences


The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014


Algebra

Discrete Dynamics in Nature and Society



Decision SciencesAdvances in

Discrete MathematicsJournal of


Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of


two sensors were used together to eliminate false positions inobject detection In Iocchi and Pellegrinirsquos paper [13] the 2Dlaser scanner and the stereo camerawere used for building the3D environment map In Lin et alrsquos paper [14] the proposedapproach combined SLAM using a 2D laser scanner anddense 3D depth information from a stereo camera and thiswork solved effectively the obstacles at different heights intothe 2D map The objective of this work is to effectively finishaccurate position based on SLAM and avoid the obstacle ofthe environment The proposed approach combines SLAMusing a laser and stereo cameras Using the cameras it canrecognize landmarks and provide obstacle information inthe environment Laser can provide high accuracy of bothrange and bearingmeasurements K-meansmethod is used toextract data from different objects Robots with the proposedmethod can have rich environment information for betternavigation and obstacle avoidance The proposed approachis implemented on an NAO humanoid robot

The rest of the paper is organized as follows In Section 2the background knowledge of laser based SLAMwith closed-loop controller the NAO platform that we have used andthe choosing method of the landmarks by using cameras areintroduced In Section 3 we briefly describe the task scenarioand K-means clustering method of extraction data fromLaser The method of extraction data from different objectsby using laser and avoidance obstacles is also described Theexperiment results are shown in Section 4 Finally Section 5gives the conclusion and discusses future work

2 Laser and Camera Based SLAM withClosed-Loop Controller

21 EKF-SLAM There are two key computational solutionsto the SLAM problem the extended Kalman filter (EKF-SLAM) and Rao-Blackwellized particle filters (FastSLAM)EKF-SLAM was presented by Smith and Cheeseman in1987 and has been used extensively [15] EKF-SLAM isvery well known to navigation problems The main stepsof SLAM include robot motion prediction new landmarksinitialization and known landmarks correction

The observation model is [7]

119910 = ℎ (119909) + V (1)

where 119909 is robot state vector and landmark states ℎ(sdot) is theobservation function and V is the measurement noise

The covariance matrix is

119875 = 1198651198751198651015840+ 119876 (2)

where 119865 is Jacobian matrix 119876 is the Gaussian noiseThe EKF correction step is written as

119911 = 119910 minus ℎ (1199091015840)

119885 = 119867119875119909119879+ 119877

(3)

where 119911 is the innovationrsquos mean 1199091015840 is the observationposition of the robot119867 is Jacobianmatrix119877 is the covariancematrix of the measurement noise

Figure 1 Aldebaran-robotics NAO humanoid

Kalman gain119870 is

119870 = 119875119867119879119885 (4)

The update position of the robot is

1199091015840= 1199091015840+ 119870119911 (5)

The correction covariance matrix is

119875 = 119875 minus 119870119885119870119879 (6)

The measurement data is given by the laser include thedistance angle between the robot and landmarks and theposition of the landmarks The extraction data method fromlaser is introduced in Section 3

22 NAO Humanoid Robot The platform selected for myresearch project is humanoid robot NAO a commonly usedhumanoid platform for education environment producedby the French company Aldebaran Robotics NAO is areasonably priced advanced humanoid robot with attractivefeatures that make it an ideal platform for research inhumanoid robots It is designed based on an open archi-tectural philosophy and can be programmed with severaldifferent programming environments NAO is a medium-sized humanoid robot developed mainly for universities andlaboratories for research and education purposes It replacedthe Sony AIBO dogs in the RoboCup Standard PlatformLeague (SPL) in 2008 As an autonomous humanoid robotNAO is capable to move in a biped way sense its closeenvironment communicate with human and think by on-board processor [16ndash21] NAO is a new biped robot recentlydeveloped by French company Aldebaran-Robotics Figure 1shows the overall appearance of the robot in our experimentNAO is 057meters high andweighs about 45 kilogramNAOwith laser head is an option provided by Aldebaran roboticsfor users intending to study specific navigation problems suchas SLAM

NAO is very light compared to other existing robotsof the same height because its BMI is about 135 (kgm2)NAO has a total of 25 degrees of freedom 11 degrees offreedom (DOF) for its lower parts and another 14 DOFfor its upper parts NAO robot is demonstrated generallyby its hardware mechanical architecture and software The















119901(119890+119879119879

119894sum119890)

else 119880 = Constant (ie 119880 = 01)



20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


e

PI controllerU

xR

xes

+

summinus


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)











3 Task Scenario

















Camerarecognize

objectYes

No

Readlaser data

Callavoidance

obstacle function


Robotmove

Lastlandmark

Yes

No





119878119894= 10038171003817100381710038171003817119909119901minus 119898119894

10038171003817100381710038171003817le10038171003817100381710038171003817119909119901minus 119898119895

10038171003817100381710038171003817 forall1 le 119895 le 119896 (7)






119898119894=110038161003816100381610038161198781198941003816100381610038161003816

sum

119909119895isin119878119894

119909119895 (8)






else walks forward



119896119901

08119879119894

55119880 01






20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




02

01

03

020 025 030

00

015

minus01


x (m)

y(m

)





Acknowledgments


References
































Volume 2014




Journal of











Journal of


Function Spaces






Algebra























119901(119890+119879119879

119894sum119890)

else 119880 = Constant (ie 119880 = 01)



20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


e

PI controllerU

xR

xes

+

summinus


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)











3 Task Scenario

















Camerarecognize

objectYes

No

Readlaser data

Callavoidance

obstacle function


Robotmove

Lastlandmark

Yes

No





119878119894= 10038171003817100381710038171003817119909119901minus 119898119894

10038171003817100381710038171003817le10038171003817100381710038171003817119909119901minus 119898119895

10038171003817100381710038171003817 forall1 le 119895 le 119896 (7)






119898119894=110038161003816100381610038161198781198941003816100381610038161003816

sum

119909119895isin119878119894

119909119895 (8)






else walks forward



119896119901

08119879119894

55119880 01






20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




02

01

03

020 025 030

00

015

minus01


x (m)

y(m

)





Acknowledgments


References
































Volume 2014




Journal of











Journal of


Function Spaces






Algebra


















3 Task Scenario

















Camerarecognize

objectYes

No

Readlaser data

Callavoidance

obstacle function


Robotmove

Lastlandmark

Yes

No





119878119894= 10038171003817100381710038171003817119909119901minus 119898119894

10038171003817100381710038171003817le10038171003817100381710038171003817119909119901minus 119898119895

10038171003817100381710038171003817 forall1 le 119895 le 119896 (7)






119898119894=110038161003816100381610038161198781198941003816100381610038161003816

sum

119909119895isin119878119894

119909119895 (8)






else walks forward



119896119901

08119879119894

55119880 01






20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




02

01

03

020 025 030

00

015

minus01


x (m)

y(m

)





Acknowledgments


References
































Volume 2014




Journal of











Journal of


Function Spaces






Algebra











Camerarecognize

objectYes

No

Readlaser data

Callavoidance

obstacle function


Robotmove

Lastlandmark

Yes

No





119878119894= 10038171003817100381710038171003817119909119901minus 119898119894

10038171003817100381710038171003817le10038171003817100381710038171003817119909119901minus 119898119895

10038171003817100381710038171003817 forall1 le 119895 le 119896 (7)






119898119894=110038161003816100381610038161198781198941003816100381610038161003816

sum

119909119895isin119878119894

119909119895 (8)






else walks forward



119896119901

08119879119894

55119880 01






20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




02

01

03

020 025 030

00

015

minus01


x (m)

y(m

)





Acknowledgments


References
































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)


20

20

15

10

05

05 10 15

00

00

minus05

minus15

minus10

minus20


x (m)

y(m

)




02

01

03

020 025 030

00

015

minus01


x (m)

y(m

)





Acknowledgments


References
































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










02

01

03

020 025 030

00

015

minus01


x (m)

y(m

)





Acknowledgments


References
































Volume 2014




Journal of











Journal of


Function Spaces






Algebra






















Volume 2014




Journal of











Journal of


Function Spaces






Algebra









research article indoor slam using laser and camera with...

Documents