Optimized motion planning of manipulators in partially-known environment using modified D* Lite algorithm

Size: px

Start display at page:

Download "Optimized motion planning of manipulators in partially-known environment using modified D* Lite algorithm"

Jean Bradford
5 years ago
Views:

1 Optmzed moton plannng of manpulators n partally-known envronment usng modfed D* Lte algorthm AMIR FEIZOLLAHI and RENE V. MAYORGA Department of Industral Systems Engneerng Unversty of Regna 3737 Wascana Parkway, Regna, Saskatchewan CANADA Fezolla@uregna.ca and Rene.Mayorga@uregna.ca Abstract: Optmzed moton plannng of the manpulators wth regards to ther energy consumpton level s a challengng problem n robotcs whch requres a combnaton of nterdscplnary studes to offer a soluton. In ths artcle, a framework s developed to desgn a moton planner mplementng a proposed search algorthm and smulate the robot moton n dfferent envronments. The superorty of the search algorthm s nvestgated and the development of the MATLAB framework s totally dscussed accompanyng the smulaton results. Key-Words: Moton Plannng, Manpulator, Optmzaton, Graph Search Algorthm, Mathematcal Modelng 1 Introducton In most ndustral and daly applcatons, manpulaton s the man or part of the task. Savng labor and reducng the cost of operaton has been always the man concern of the desgners to mnmze the tme and effort needed to perform these tasks. In many applcatons such as exploraton n the dangerous envronment [1] and rescue operaton n natural dsasters [2], to dexterous manpulaton [3], and pont-to-pont placement n ndustral workspaces [4], usng human operator can be ether neffcent or dangerous. Robot manpulators, as one of the man group of robots, are desgned from the sequence of rgd or/and contnuum lnks, actuators, and endeffector to mnmze or entrely elmnate human nteracton wth the surroundng envronment n dfferent workspaces and condtons. Developng an optmzed algorthm to convert a hgh-level task from human nto a low-level descrpton understandable by the robot has been always one of the most nterestng research topcs n robotcs. Moton plannng s a step-by-step procedure that generates the most approprate moton for the robot based on the modelng, analyzng the possble motons (robot s workspace), and the specfed crtera. Moton plannng can be generally classfed nto two groups: moton plannng n statc envronment and moton plannng n a dynamc envronment. In a statc envronment, all the nformaton about the obstacles, avodng crtera, workspace features, and all other varables n the defned cost functon are obtanable n pre-processng phase. The moton plannng procedure n ths type of problems nvolves analyzng the workspace n order to reach the goal node whle avodng the obstacles. Another type of moton plannng algorthms deals wth the problems n whch part of the workng envronment s unknown (partally-known envronment), or the robot s explorng the envronment for the frst tme (unknown envronment), or some of the envronment features lke locaton of the obstacles and the goal node can change (dynamc envronment). The algorthms that are developed to solely solve the dynamc envronment moton plannng problems are manly based on adaptve path plannng where the pror nformaton about the envronment s used to generate the best path [5]. In an attempt to deal wth the problems of partally-known envronment, D* algorthm was proposed n [6]. D* s an nformed ncremental graph search algorthm that has been wdely used for the automatc navgaton of the moble robots. D* Lte [7] whch s also an ncremental heurstc search algorthm, s bult on LPA* to determne the best path from the start node to the goal node whle the path costs change due to dscoverng new obstacles or changes n obstacle features. More detals about ths algorthm, the procedure of ts mplementaton, and the proposed modfcaton to enhance ts performance can be found n the followng sectons of ths artcle. E-ISSN: Volume 16, 2017

2 Optmzed moton plannng of the manpulators wth regards to the consumed energy of the actuators takes ths problem to a new level n whch modelng the robot and ts actuators as a unform system play a bg role n the moton plannng procedure. In ths artcle, a novel method s used to combne the dynamcs modelng of the robot and ts actuators to develop a sngle state space descrbng the moton of the robot n order to mnmze the energy consumpton of the manpulator durng ts moton from the start node to the goal node whle avodng the obstacles. In ths study, a modfcaton to D* Lte algorthm s proposed, the proposed algorthm s mplemented usng MATLAB and the correspondng results are presented. Ths artcle s organzed as follows: the governng equaton of the robot-actuator system s derved n secton 2 followng by a dscusson on the development of modfed D* Lte search algorthm n secton 3. The procedure of mplementng the graph search algorthm usng MATLAB s dscussed and the correspondng results are presented n sectons 4 and 5. 2 Mathematcal Modelng of the Robot As mentoned n the ntroducton secton of ths artcle, the mathematcal model of the robot and ts equaton of moton should be developed to calculate the energy consumpton durng ts moton along the desred trajectory. Usng Hamlton s prncple [8] for a conservatve system between two states from t 1 to t 2, there s a statonary value for the ntegral n (1) where L s a calculable form (2) n whch T and V are the knetc and potental energy of the system, respectvely. I t1 = Ldt (1) t2 Lqq (,,t) = Tqq (, ) Vq ( ) (2) Takng the dervatve of both sdes n (1) yelds the Lagrange equaton of moton for the system: d T T V (3) + = 0 dt q q q Rewrtng (3) n exstence of external forces such as the appled torques by the actuators at the robot s jonts, T Dqq ( ) + qcqq ( ) + Gq ( ) = τ (4) In whch D and C matrces can be determned usng (5) and (6), respectvely. n T T T D= [m JV J ] V + J ω R oir o J ω (5) = 1 C = ( d ) ( d ) ( d ) jk + + kj k j q q q j k Where R s the rotaton matrx and J s the Jacoban matrx that relates the jont angular veloctes to the velocty of any pont along the robot s lnks and vce versa. DC motors are wdely used as the actuator n many control systems. They can be drectly connected to the wheels or cables to provde rotary or lnear moton. The appled voltage to the armature (V) s the nput of the DC motor and the angular velocty of the shaft s the output. Assumng that the rotor and the shaft are rgd and also wth the assumpton of proportonal vscous frcton to the shaft s angular velocty, usng Newton s second law and Krchhoff s voltage law yeld t (6) J + B + τ = K (7) d L + R = V K e (8) dt Combnng (4) and (7), solatng rewrtng the equatons n matrx form: d C( ) B K t = V NGT dt + + J D( ) J D( ) L Ke R 0 L L n (8), and In whch (nlnear Gravtatonal Terms) can be calculated usng (10). (9) 0 G( ) NGT = J + D( ) (10) 0 Solvng the dfferental equatons of moton n (9) that are assocated wth the dynamcs of the robot results n the calculaton of energy consumpton as a varable n the cost functon of the graph search algorthm. The governng equatons of moton of the robot are solved smultaneously usng the fourth order of Runge-Kutta method n MATLAB. Ths soluton wll gve a great close-to-realty approxmaton of the system s behavor and returns the needed varables for computaton of energy consumpton of the robot. E-ISSN: Volume 16, 2017

3 3 Graph Search Algorthm The graph search algorthms can be generally categorzed nto two groups of ncremental search algorthms and heurstc search algorthms. The frst one uses the nformaton n the prevous searches to solve the current search problem whle the latter one uses s manly based on A* algorthm, n whch the heurstc nformaton of each node s taken nto consderaton to focus the search on reachng the goal node. D* Lte s an ncremental heurstc search algorthm that uses the man features of both methods to speed up the search whle puttng the prorty on reachng the goal node. D* Lte s bascally the ncremental form of A* algorthm for searchng n an unknown envronment. D* Lte algorthm uses the same navgaton method as D* but consderably shorter. It s as effcent as D* but dfferent n terms of the algorthm. These features make D* Lte one of the best search algorthms for unknown/partally-known envronment [7]. D* Lte assgns two estmates of cost n each node, g and rhs. The value of g corresponds to the objectve functon and rhs s the one-step lookforward value of the objectve functon. Ths algorthm also defnes the status of consstency for each node. The node s Consstent f g=rhs Inconsstent f g rhs The Inconsstent nodes on the open lst have the prorty to be checked. The rhs value of each node s computable usng the g value of ts successor nodes and the path cost to those nodes usng (11). rhs ( u) = mn p Succ( u) ( cost ( u, p) + g ( p) ) (11) The key value of a node sets the prorty of each node on the open lst. Ths value s the summaton of the heurstc value of the node, h, and mnmum of ts g and rhs value. Accordng to (12), n the case of te, the secondary key value s consdered to break the te. mn ( g ( u), rhs ( u) ) + h( start, u) key( u) = (12) mn ( g ( u), rhs ( u) ) D* Lte algorthm conssts of fve procedures: Intalzaton, Man Procedure, Key Value Calculaton, de Update, and Best Path Generaton [7]. Intalzaton ncludes creatng the open lst, assgnng a relatvely bg value to the g and rhs of all the nodes and fnally nsertng the goal node to the open lst. The process of fndng the best path n D* Lte algorthm, unlke A*, starts wth the goal node. Key Value Calculaton procedure nvolves all the computatons that are needed to assgn the key value to the nput node. The formulaton for ths procedure s based on (12). de Update procedure bascally sets the rhs value of the node under process and returns t to the approprate lst of nodes. In Best Path Computaton, the consstency status of the nodes wth the hgher key values on the open lst s determned and the approprate functon s called accordngly to change the status of the node f t s needed. If the node s under-consstent then a functon s called to change the node to overconsstent status. These changes wll be later propagated to the neghborng nodes of the node under process. The core procedure of D* Lte s Man that has to be run repeatedly to execute the best-path-fndng and scannng functons untl the best path between the start node and the goal node s generated. Runnng D* Lte algorthm, best path can be obtaned by followng the gradent of g values from the start node. Ths means that the g values of all the neghborng nodes to each node should be compared to each other and sorted from the start node to the goal node to make sure that the path wth the mnmum cost s chosen as the best path. D* Lte s an algorthm that s capable of solvng the graph search problems wth consderably hgh level of effcency. However, a smple modfcaton to ts best path generaton method ncreases the effcency of ths algorthm sgnfcantly. Store the Best Neghbour de New de Update Procedure Start u goal rhs(u) [Equaton 2-34] u ϵ U U.Remove(u) g(u) rhs(u) U.Insert(u,key(u)) Fg.1 - Modfcaton to D* Lte Algorthm, de Update procedure End E-ISSN: Volume 16, 2017

4 Addng a store functon n the de Update procedure of D* Lte, there would be no need to execute an extra procedure for generatng the best path n the post-processng phase of the graph search. In Fg. 1, an event (the dashed ellpse) s added to the orgnal procedure under the name of Store the Best Neghbor de. By addng ths functon to the procedure, after calculatng the rhs value of the under process node, a new property wll be created to store the best neghbor of the node n terms of rhs calculaton. In new de Update Procedure, generaton of the best path s smply plng up the nodes from the start node to the goal node based on the new property added to the nodes. 4 Developng the Smulaton Framework The prevous sectons provded the foundatons for modelng the robot and also delvered the theoretcal structure for the proposed search algorthm n partally-known envronment. In order to mplement the search algorthm on a robot wth user-defned characterstcs, a MATLAB framework s desgned to model the robot, apply the search algorthm, and smulate the robot moton n dfferent scenaros. The framework conssts of thousands of lnes of MATLAB scrpt n several classes wth dfferent types of functons and propertes to execute the above-mentoned tasks. The overall steps for desgnng such framework can be summarzed as developng the below classes: * Robot dynamcs, a class that contans several functons and propertes for defnng the robot, developng the correspondng equatons and dong the needed calculaton based on the formulaton provded n secton 2. * Motor dynamcs, a class that s responsble for developng the equaton of moton of the userdefned actuator, solvng the coupled dfferental equatons and calculatng the energy consumpton upon runnng the graph search algorthm * Occupancy analyss, group of classes and subclasses for generatng random confguraton of the manpulators and dentfyng the workspace accordngly * Search algorthm, the class that s developed to mplement the proposed search algorthm and fnd the best path wth mnmum energy consumpton * Robot smulaton, a class that deals wth the robot moton smulaton and prepares an approprate graphc output to present the results There are some assumptons assocated wth the development of the robot dynamcs and motor dynamcs classes. It s assumed that the actuators actng on each jont are sngle functon addng only one degree of freedom to the manpulator. In another word, the number of degrees of freedom of the robot s equal to the number of the robot s lnks. Although the user can delberately change the robot characterstcs, ths assumpton converts the moton plannng of the robot to a more complcated problem snce the robot has to reach the goal node wth more restrcted lmts. It s also assumed that the magnetc feld of the DC motors at the jonts s constant, the flux s assumed to flow straght nto the motor and t can be formulated as ndcated n (7). It s also assumed that the actuators have a selflock mechansm that causes the full stop n actuators moton once the robot reaches the ntended confguraton. In Motor Dynamcs class, all the lnks features such as length, mass, and confguraton are stored n the assocated cell arrays and sent to the symbolc method n Robot Dynamcs class n whch the correspondng equatons of moton are developed. The dfferental equatons are then returned to Motor Dynamcs class to do the fnal calculatons for the energy consumpton. The man process n the graph search procedure usng D* Lte algorthm s to dentfy the nodes that are to be checked, labelng them to be lsted under Open Lst or Free des Lst, and sortng them based on the defned cost functon. The process starts wth calculatng the key value of the Goal node s neghbors and puttng them on the Open Lst. Based on the key value, the next node wll be selected to be checked. The Goal node s then transferred to Free des Lst and all ts neghborng nodes are on the Open Lst. Followng ths procedure, removng the processed nodes from the Open Lst and addng them to the Free des Lst, the graph search algorthm reaches End once the Start node s added to the Free des Lst (see Fg. 2). A MATLAB class, Plot_2D, s developed to deal wth the smulaton of the robot moton and dsplayng the output result from the prevously dscussed classes. Upon fndng the best path usng the search algorthm and the requred MATLAB objects, the plottng class smulates the robot moton. Ths class also provdes a graphc output showng the jonts rotaton and energy consumpton graphs durng the robot moton. E-ISSN: Volume 16, 2017

5 Start Start and Goal des Add to Free des lst Current node ntalzaton Pop the neghbours rhs calcualaton rhs changed On Open Lst Best Neghbour = Current de On Free des Lst (a) Add to Open Lst Sort key Loop Breaker Condton Start node on Free des Lst Store Free des lst End Fg. 2 - Workflow of the modfed D* Lte algorthm (b) 5 Results In the prevous sectons, the mathematcal modelng, mplementaton of the search algorthm, and developng the MATLAB framework have been dscussed. In ths secton, some typcal manpulaton problems are defned and the search algorthm s appled to fnd the optmzed path for the manpulaton task. The performance of A* and modfed D* Lte algorthms are compared wth each other and the re-plannng procedure results are dscussed. One of the best and easest ways to nvestgate the effcency of two search algorthms s comparng ther processng tme for fndng the best path. For ths purpose, the proposed algorthm (modfed D* Lte), D* Lte and A* are appled to exactly the same problems. Fve dfferent scenaros wth fve dfferent start-to-goal path length are defned and both algorthms are appled to generate the best path between two gven nodes. The outputs, as the best path are exactly the same. Scenaro Table. 1 - Comparson between MPD* Lte, D* Lte, and A* algorthms: processng tme Total des Best Path Length Processng Tme (s) MPD* Lte D* Lte A* (c) (d) Fg. 3 - MPD* Lte, D* Lte, and A* graphs: a) MPD* Lte algorthm: processng tme graph b) D* Lte algorthm: processng tme graph c) A* Lte algorthm: processng tme graph d) Comparatve graph for fve smulated scenaros Accordng to the results presented n Table. 1 and Fg. 3, t can be nferred that both open lst sze and processng tme, whch are bascally the governng factors that affect computatonal complexty of the algorthms, have exponental relatonshp wth the number of nodes (namely number of all nodes and the length of the best path between the start node and the goal node) n A* E-ISSN: Volume 16, 2017

algorthm. On the other hand, the computatonal complexty of D* Lte and MPD* Lte algorthms, accordng to the same results, has a polynomal relatonshp wth the number of nodes.

For modelng the robot s actuators, DC motors are used as dscussed n secton 2. It s assumed that all the jonts have the same actuators wth smlar characterstcs.

To verfy the capablty of the proposed search algorthm n fndng the optmzed path, a workspace s defned as the one n Fg. 4.

6 algorthm. On the other hand, the computatonal complexty of D* Lte and MPD* Lte algorthms, accordng to the same results, has a polynomal relatonshp wth the number of nodes. Although ths polynomal relatonshp can be dfferent for these two algorthms (e.g. dfferent order of polynomal relatonshp), they both perform much faster than A* graph search algorthm. For modelng the robot s actuators, DC motors are used as dscussed n secton 2. It s assumed that all the jonts have the same actuators wth smlar characterstcs. One of the typcal manpulaton problems for robotc arms s ther maneuverablty n a tghtly crowded workspace. To verfy the capablty of the proposed search algorthm n fndng the optmzed path, a workspace s defned as the one n Fg. 4. Sx obstacles n dfferent shapes and szes are located to lmt the robot s workspace. The robot s msson s to safely manpulate an object from the rght corner of the workspace and drop t n another corner between two obstacles whle avodng any collson wth them. The manpulator s defned to have four lnks of the same length. The energy consumpton graph, the processng tme and the best path are depcted n Fg. 4. Fg. 4 - Energetcally optmzed path for the manpulator n a crowded workspace To verfy the re-plannng procedure of the search algorthm, the robotc arm s located n a partallyknown envronment where some of the obstacles are pre-defned. The manpulator detects a new obstacle durng ts moton toward the goal node. As a result, all the nodes neghborng the new obstacle wll have an update on ther cost and the search algorthm takes the re-plannng procedure to fnd another path wth mnmum energy consumpton. The correspondng result and more detals on ths scenaro can be found n Fg. 5. Fg. 5 - Re-plannng usng the modfed D* Lte algorthm n case of detectng new obstacle 6 Conclusons Usng Manpulators n dfferent szes and shapes for delverng a wde range of tasks from smple repettve manpulaton to performng the mantenance procedure n hazardous workspaces, has been always an nterestng and at the same tme challengng problem n the feld of robotcs. In ths study, the optmzed moton plannng of the manpulators n partally-known envronment was addressed and a search algorthm was proposed and appled as the soluton to ths problem. The mathematcal modelng of the robot and ts actuators was done and the correspondng formulaton was derved. D* Lte algorthm as one of the most wellknown search algorthms was dscussed, ts superorty over A* algorthm was studed and a modfcaton was proposed to enhance ts effcency. Ths algorthm was mplemented usng a MATLAB framework for generatng the best path for the userdefned manpulators n dfferent scenaros. Although there may be much more possble scenaros to study the effcency of the proposed moton planner, the selected results from tens of nvestgated scenaros are the most concse and nformatve cases. E-ISSN: Volume 16, 2017

7 References: [1] K. S. Senthlkumar and K. K. Bharadwaj, Mult-robot exploraton and terran coverage n an unknown envronment, Robotcs and Autonomous Systems, vol. 60, no. 1, pp , [2] K. Nagatan, S. Krbayash, Y. Okada, S. Tadokoro, T. Nshmura, T. Yoshda, E. Koyanag, and Y. Hada, Redesgn of rescue moble robot Qunce, IEEE Internatonal Symposum on Safety, Securty, and Rescue Robotcs (SSRR), 2011, pp [3] P. Sabetan, A. Fezollah, F. Cheraghpour, and S. A. A. Moosavan, A compound robotc hand wth two under-actuated fngers and a contnuous fnger, IEEE Internatonal Symposum on Safety, Securty, and Rescue Robotcs (SSRR), 2011, pp [4] M. Hvlshøj, S. Bøgh, O. S. Nelsen, and O. Madsen, Autonomous ndustral moble manpulaton (AIMM): past, present and future, Industral Robot, vol. 39, no. 2, pp , [5] J. van den Berg, D. Ferguson, and J. Kuffner, Anytme path plannng and replannng n dynamc envronments, IEEE Internatonal Conference on Robotcs and Automaton (ICRA), 2006, pp [6] A. Stentz, Optmal and effcent path plannng for partally-known envronments, IEEE Internatonal Conference on Robotcs and Automaton (ICRA), 1994, pp [7] S. Koeng and M. Lkhachev, D*Lte, Eghteenth Natonal Conference on Artfcal Intellgence, Amercan Assocaton for Artfcal Intellgence, 2002, pp [8] H. Goldsten, Classcal mechancs. Pearson Educaton Inda, E-ISSN: Volume 16, 2017

A Binarization Algorithm specialized on Document Images and Photos

A Binarization Algorithm specialized on Document Images and Photos A Bnarzaton Algorthm specalzed on Document mages and Photos Ergna Kavalleratou Dept. of nformaton and Communcaton Systems Engneerng Unversty of the Aegean kavalleratou@aegean.gr Abstract n ths paper, a