Patent Grant
10429847
This application is related to U.S. application Ser. No. 15/712,222 filed Sep. 22, 2017 entitled “Multi-Resolution Scan Matching With Exclusion Zones”, which is incorporated herein by reference.
The invention described herein relates generally to robot navigation along a goal path toward a target location in the presence of moving and stationary obstacles. Specifically, the present invention is an improved method for determining the trajectory of an incremental movement of a robot that avoids collisions with obstacles, while maximizing progress along the goal path.
In many applications, robots are used to perform functions in place of humans or to assist humans in order to increase productivity and efficiency. One such application is order fulfillment, which is typically performed in a large warehouse filled with products to be shipped to customers who have placed their orders over the internet for home delivery. Fulfilling such orders in a timely, accurate and efficient manner is logistically challenging to say the least.
In an online Internet shopping application, for example, clicking the “check out” button in a virtual shopping cart creates an “order.” The order includes a listing of items that are to be shipped to a particular address. The process of “fulfillment” involves physically taking or “picking” these items from a large warehouse, packing them, and shipping them to the designated address.
An important goal of the order fulfillment process is thus to ship as many items in as short a time as possible. The process of receiving an order, planning its fulfillment, finding the storage shelf or bin, picking the product, and repeating the process for each item on the order, then delivering the order to a shipping station is repetitive and labor intensive. In a warehouse stocked with thousands or tens of thousands of items of rapidly turning inventory, robots play a critical role in ensuring timely and efficient order fulfillment. In addition, the products that will ultimately be shipped first need to be received in the warehouse and stored or “placed” in storage bins in an orderly fashion throughout the warehouse so they can be readily retrieved for shipping.
Using robots to perform picking and placing functions may be done by the robot alone or with the assistance of human operators. Picking and placing or stocking functions, whether or not performed with human interaction, requires that the robot navigate from its present location to a target product storage or “bin” location. Along the robot's goal path from present location to product storage bin, the robot typically encounters stationary and moving obstacles such as walls, shelves, support structure, humans and other robots. Furthermore, as new product is stocked and depleted, as new shelves and bins are added and removed, and as miscellaneous objects are introduced into the shared human-robot space, the dynamic nature of an order fulfillment warehouse requires constant updating of information about the warehouse and its contents.
Obstacle avoidance while navigating the robot's goal path involves computing a series of increment movements using information on nearby fixed and moving obstacles. The incremental movement must not drive the robot into an obstacle, fixed or moving, and the trajectory of the robot to drive its movement must be computed within a fixed cycle time. Known methods of robot navigation, however, choose between approaches better suited to navigating fixed obstacles and approaches better suited for navigating moving obstacles, i.e. robots. What is needed is a computationally efficient method for robot navigation considering both moving and fixed obstacles, thus improving the ability of the robot to make progress toward its target location in the allotted cycle time for each increment movement.
The benefits and advantages of the present invention will be readily apparent from the Brief Summary of the Invention and Detailed Description to follow. One skilled in the art will appreciate that the present teachings can be practiced with embodiments other than those summarized or disclosed below.
In a first aspect, there is a method for navigation of a robot along a goal path and avoiding obstacles. The method includes receiving a goal pose for a first robot, determining a goal path for the first robot, receiving an obstacle map, receiving the pose of the first robot, receiving the pose of one or more other robots, generating a set of candidate velocities for the first robot, evaluating using a first objective function the first set of candidate velocities, selecting, based on the first objective function, a first preferred velocity of the first robot, creating a set of velocity obstacles based on the pose(s) of the one or more other robots and the first preferred velocity of the first robot, evaluating using a second objective function the set of candidate velocities selecting based on the second objective function a second preferred velocity for the first robot, and moving the first robot based on the second preferred velocity.
In one embodiment, the goal path may be a path from a current pose of the first robot to the goal pose of the first robot. The goal pose of the robot may be the pose of a fiduciary associated product bin in an order fulfillment warehouse application.
In some embodiments, the pose of the first robot may be determined by one or more of many-to-many multiresolution scan matching (M3RSM), adaptive monte carlo localization (AMCL), geo-positioning satellite (GPS), fiducial information, and odometry-based on robot sensors.
In a preferred embodiment, generating the set of candidate velocities for the first robot includes assuming a candidate velocity over one or more time steps applying motion, obstacle, and inertial constraints to generate only candidate velocities having admissible trajectories.
In another embodiment, the first objective function is comprised of one or more cost functions of the form G(v,ω)=α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω), where G(v,ω) is the objective function, α, β, γ are weights; heading(v,ω) is a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle (its “clearance”); and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω). The first objective function may further include one or more of a path cost function which scores how much the candidate velocity would radiate from the goal path; an obstacle cost function scoring proximity to obstacles; or an oscillation cost function assigning higher costs to changes in rotational velocity from a previous preferred velocity. The cost functions of the first objective function may invalidate a candidate velocity by assigning a highest cost score to the candidate velocity.
In yet another embodiment, creating the set of velocity objects includes converting the preferred velocity from a non-holonomic to a holonomic velocity. Converting the preferred velocity to a holonomic velocity may include increasing the radius of the one or more other robots by a maximum distance between a preferred trajectory and a straight-line trajectory.
In a preferred embodiment, the second objective function is comprised of one or more cost functions of the form ORCA/DWA=CDWA+αORCA*CORCA, where CDWA is defined as CDWA=α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω) with α, β, γ as weights; heading(v,ω) a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle; and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω), and where CORCA is defined as CORCA=αv (vt-vpref)+penalty+αd*d (P, vt), where αd and αv are weights; vt is a candidate velocity being evaluated; vpref is the preferred velocity; P is the polygon formed by the union of VOs; d (P, vt) is a measure of how much a candidate velocity violates the VOs; and penalty is a penalty cost imposed when a candidate velocity vt violates a VO. Further, cost function d (P, vt) is a function of the minimum distance from the perimeter of polygon P to a point defined by the trajectory t reached by candidate velocity vt.
In a second aspect of the invention, there is a robot system for navigation of a robot along a goal path and avoiding obstacles, including a transceiver, a data storage device, a data processor and a data storage device having instructions stored thereon for execution by the data processor. The instructions stored thereon instruct the robot system to receive a goal pose for a first robot, determining a goal path for the first robot, receive an obstacle map, receive the pose of the first robot, receive the pose of one or more other robots, generate a set of candidate velocities for the first robot, evaluate using a first objective function the first set of candidate velocities, select based on the first objective function a first preferred velocity of the first robot, create a set of velocity obstacles based on the pose(s) of the one or more other robots and the first preferred velocity of the first robot, evaluate using a second objective function the set of candidate velocities selecting based on the second objective function a second preferred velocity for the first robot, and move the first robot based on the second preferred velocity.
In one embodiment of this second aspect, the goal path may be a path from a current pose of the first robot to the goal pose of the first robot. The goal pose of the robot may be the pose of a fiduciary associated product bin in an order fulfillment warehouse application.
In a preferred embodiment, generating the set of candidate velocities for the first robot includes assuming a candidate velocity over one or more time steps applying motion, obstacle, and inertial constraints to generate only candidate velocities having admissible trajectories.
In another embodiment, the first objective function is comprised of one or more cost functions of the form G(v,ω)=α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω), where G(v,ω) is the objective function, α, β, γ are weights; heading(v,ω) is a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle (its “clearance”); and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω). The first objective function may further include one or more of a path cost function which scores how much the candidate velocity would radiate from the goal path; an obstacle cost function scoring proximity to obstacles; or an oscillation cost function assigning higher costs to changes in rotational velocity from a previous preferred velocity. The cost functions of the first objective function may invalidate a candidate velocity by assigning a highest cost score to the candidate velocity.
In yet another embodiment, creating the set of velocity objects includes converting the preferred velocity from a non-holonomic to a holonomic velocity. Converting the preferred velocity to a holonomic velocity may include increasing the radius of the one or more other robots by a maximum distance between a preferred trajectory and a straight-line trajectory.
In a preferred embodiment, the second objective function is comprised of one or more cost functions of the form ORCA/DWA=CDWA+αORCA*CORCA, where CDWA is defined as CDWA=α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω) with α, β, γ as weights; heading(v,ω) a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle; and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω), and where CORCA is defined as CORCA=αv (vt-vpref)+penalty+αd*d (P, vt), where αd and αv are weights; vt is a candidate velocity being evaluated; vpref is the preferred velocity; P is the polygon formed by the union of VOs; d (P, vt) is a measure of how much a candidate velocity violates the VOs; and penalty is a penalty cost imposed when a candidate velocity vt violates a VO. Further, cost function d (P, vt) is a function of the minimum distance from the perimeter of polygon P to a point defined by the trajectory t reached by candidate velocity vt.
In a third aspect of the invention, there is a robot system including a plurality of robots under the supervision of a supervisory system for performing the methods of the first aspect.
Embodiments of the present invention will now be described, by way of example only, with reference to the attached Figures, wherein:
The invention described herein is directed to methods and systems for use with an autonomous or semi-autonomous robot for improved navigation of the robot from a current location to a target location along its “goal path” within an environment containing obstacles and free space. Specifically, the methods and systems of the present invention provide a computationally efficient improvement over the prior art for accurately determining the next instantaneous velocity to apply to the robot's propulsion control using a combination of constraint-based obstacle avoidance methods.
The disclosure and the various features and advantageous details thereof are explained more fully with reference to the non-limiting embodiments and examples that are described and/or illustrated in the accompanying drawings and detailed in the following description. It should be noted that the features illustrated in the drawings are not necessarily drawn to scale, and features of one embodiment may be employed with other embodiments as the skilled artisan would recognize, even if not explicitly stated herein. Moreover, it is noted that like reference numerals represent similar parts throughout the several views of the drawings.
Descriptions of well-known components and processing techniques may be omitted so as to not unnecessarily obscure the embodiments of the disclosure. The examples used herein are intended merely to facilitate an understanding of ways in which the disclosure may be practiced and to further enable those of skill in the art to practice the embodiments of the disclosure. Accordingly, the examples and embodiments herein should not be construed as limiting the scope of the disclosure.
One skilled in the art will appreciate that the present teachings can be practiced with embodiments other than those disclosed. While the description provided herein is focused on picking items from bin locations in the warehouse to fulfill an order for shipment to a customer, the system is equally applicable to the storage or placing of items received into the warehouse in bin locations throughout the warehouse for later retrieval and shipment to a customer. The invention is also applicable to inventory control tasks associated with such a warehouse system, such as, consolidation, counting, verification, inspection and clean-up of products.
The methods and systems of the present invention may also be applied in other types of environments with other types of obstacles for other types of applications. Any physical object or structure, stationary or dynamic, may be considered an “obstacle” in an application of the present invention. Obstacles may further include humans and other robots operating within the environment, and the location of the humans and other robots may be current locations or target locations in the performance of cooperative tasks. Target locations may include one or more locations within the environment for positioning one or more robots to perform or to assist a human in the performance of a task or succession of tasks.
These and other benefits and advantages will become readily apparent from the examples and illustrations described below.
Referring to
In a preferred embodiment, a robot 18, shown in
Referring again to
Although robot 18 excels at moving around the warehouse 10, with current robot technology, it is not very good at quickly and efficiently picking items from a shelf and placing them in the tote 44 due to the technical difficulties associated with robotic manipulation of objects. A more efficient way of picking items is to use a local operator 50, which is typically human, to carry out the task of physically removing an ordered item from a shelf 12 and placing it on robot 18, for example, in tote 44. The robot 18 communicates the order to the local operator 50 via the tablet 48 (or laptop/other user input device), which the local operator 50 can read, or by transmitting the order to a handheld device used by the local operator 50.
Upon receiving an order 16 from the order server 14, the robot 18 proceeds to a first warehouse location, e.g. as shown in
Upon reaching the correct location, the robot 18 parks itself in front of a shelf 12 on which the item is stored and waits for a local operator 50 to retrieve the item from the shelf 12 and place it in tote 44. If robot 18 has other items to retrieve it proceeds to those locations. The item(s) retrieved by robot 18 are then delivered to a packing station 100,
It will be understood by those skilled in the art that each robot may be fulfilling one or more orders and each order may consist of one or more items. Typically, some form of route optimization software would be included to increase efficiency, but this is beyond the scope of this invention and is therefore not described herein.
In order to simplify the description of the invention, a single robot 18 and operator 50 are described. However, as is evident from
The navigation approach of this invention, as well as the semantic mapping of a SKU of an item to be retrieved to a fiducial ID/pose associated with a fiducial marker in the warehouse where the item is located, is described in detail below with respect to
Using one or more robots 18, a map of the warehouse 10 must be created and dynamically updated to determine the location of objects, both static and dynamic, as well as the locations of various fiducial markers dispersed throughout the warehouse. To do this, one of the robots 18 navigate the warehouse and build/update a map 10a,
Order Fulfillment
Robot 18 utilizes its laser radar 22 to create/update map 10a of warehouse 10 as robot 18 travels throughout the space identifying open space 112, walls 114, objects 116, and other static obstacles such as shelves 12a in the space, based on the reflections it receives as the laser radar scans the environment.
While constructing the map 10a or thereafter, one or more robots 18 navigates through warehouse 10 using cameras 24a and 24b to scan the environment to locate fiducial markers (two-dimensional bar codes) dispersed throughout the warehouse on shelves proximate bins, such as 32 and 34,
By the use of wheel encoders and heading sensors, vector 120, and the robot's position in the warehouse 10 can be determined. Using the captured image of a fiducial marker/two-dimensional barcode and its known size, robot 18 can determine the orientation with respect to and distance from the robot of the fiducial marker/two-dimensional barcode, vector 130. With vectors 120 and 130 known, vector 140, between origin 110 and fiducial marker 30, can be determined. From vector 140 and the determined orientation of the fiducial marker/two-dimensional barcode relative to robot 18, the pose (position and orientation) defined by x,y,z coordinates relative to origin 110 and by a quaternion (x, y, z, ω) for fiducial marker 30 can be determined. A discussion of using quaternions to represent and effect orientations is found in Berthold K. P. Horn, “Closed-form solution of absolute orientation using unit quaternions,” Journal of the Optical Society of America, 4(4), April 1987, pp. 629-642, which is incorporated by reference herein. One skilled in the art would recognize that other coordinate systems and techniques for determination of fiducial marker position and orientation may be used, and that pose may determine an absolute or relative position and/or orientation from an arbitrary origin.
Flowchart 200,
In look-up table 300, which may be stored in the memory of each robot, there are included for each fiducial marker a fiducial identification, 1, 2, 3, etc., and a pose for the fiducial marker/bar code associated with each fiducial identification. The pose consists of the x,y,z coordinates in the warehouse relative to origin 110, along with the orientation or the quaternion (x,y,z,ω).
In another look-up Table 400,
The alpha-numeric bin locations are understandable to humans, e.g. operator 50,
The order fulfillment process according to this invention is depicted in flowchart 500,
Item specific information, such as SKU number and bin location, obtained by the warehouse management system 15, can be transmitted to tablet 48 on robot 18 so that the operator 50 can be informed of the particular items to be retrieved when the robot arrives at each fiducial marker location.
With the SLAM map and the pose of the fiducial ID's known, robot 18 can readily navigate to any one of the fiducial ID's using various robot navigation techniques. The preferred approach involves setting an initial route to the fiducial marker pose given the knowledge of the open space 112 in the warehouse 10 and the walls 114, shelves (such as shelf 12) and other obstacles 116. As the robot begins to traverse the warehouse using its laser radar 22, it determines if there are any obstacles in its path, either fixed or dynamic, such as other robots 18 and/or operators 50, and iteratively updates its path to the pose of the fiducial marker. The robot re-plans its route about once every 50 milliseconds, constantly searching for the most efficient and effective path while avoiding obstacles.
Generally, localization of the robot within warehouse 10a is achieved by many-to-many multiresolution scan matching (M3RSM) operating on the SLAM map. Compared to brute force methods, M3RSM dramatically reduces the computational time for a robot to perform scan matching for determining the robot's current pose. A discussion of M3RSM can be found in Edwin Olson, “M3RSM: Many-to-many multi-resolution scan matching”, Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), June 2015, which is incorporated herein by reference. Robot localization is further improved by minimizing the M3RSM search space according to methods disclosed in related U.S. patent application Ser. No. 15/712,222, which is incorporated by reference herein.
With the product SKU/fiducial ID to fiducial pose mapping technique combined with the SLAM navigation technique both described herein, robots 18 are able to very efficiently and effectively navigate the warehouse space without having to use more complex navigation approaches, which typically involve grid lines and intermediate fiducial markers to determine location within the warehouse.
Generally, navigation in the presence of other robots and moving obstacles in the warehouse is achieved by collision avoidance methods including the dynamic window approach (DWA) and optimal reciprocal collision avoidance (ORCA). DWA computes among feasible robot motion trajectories an incremental movement that avoids collisions with obstacles and favors the desired path to the target fiducial marker. ORCA optimally avoids collisions with other moving robots without requiring communication with the other robot(s). Navigation proceeds as a series of incremental movements along trajectories computed at the approximately 50 ms update intervals. Collision avoidance may be further improved by techniques described herein.
Robot System
Data processor 620, processing modules 642 and sensor support modules 660 are capable of communicating with any of the components, devices or modules herein shown or described for robot system 600. A transceiver module 670 may be included to transmit and receive data. Transceiver module 670 may transmit and receive data and information to and from a supervisory system or to and from one or other robots. Transmitting and receiving data may include map data, path data, search data, sensor data, location and orientation data, velocity data, and processing module instructions or code, robot parameter and environment settings, and other data necessary to the operation of robot system 600.
In some embodiments, range sensor module 662 may comprise one or more of a scanning laser radar, laser range finder, range finder, ultrasonic obstacle detector, a stereo vision system, a monocular vision system, a camera, and an imaging unit. Range sensor module 662 may scan an environment around the robot to determine a location of one or more obstacles with respect to the robot. In a preferred embodiment, drive train/wheel encoders 664 comprises one or more sensors for encoding wheel position and an actuator for controlling the position of one or more wheels (e.g., ground engaging wheels). Robot system 600 may also include a ground speed sensor comprising a speedometer or radar-based sensor or a rotational velocity sensor. The rotational velocity sensor may comprise the combination of an accelerometer and an integrator. The rotational velocity sensor may provide an observed rotational velocity for the data processor 620, or any module thereof.
In some embodiments, sensor support modules 660 may provide translational data, position data, rotation data, level data, inertial data, and heading data, including historical data of instantaneous measures of velocity, translation, position, rotation, level, heading, and inertial data over time. The translational or rotational velocity may be detected with reference to one or more fixed reference points or stationary objects in the robot environment. Translational velocity may be expressed as an absolute speed in a direction or as a first derivative of robot position versus time. Rotational velocity may be expressed as a speed in angular units or as the first derivative of the angular position versus time. Translational and rotational velocity may be expressed with respect to an origin 0,0 (e.g.
In some embodiments, robot system 600 may include a GPS receiver, a GPS receiver with differential correction, or another receiver for determining the location of a robot with respect to satellite or terrestrial beacons that transmit wireless signals. Preferably, in indoor applications such as the warehouse application described above or where satellite reception is unreliable, robot system 600 uses non-GPS sensors as above and techniques described herein to improve localization where no absolute position information is reliably provided by a global or local sensor or system.
In other embodiments, modules not shown in
One skilled in the art would recognize other systems and techniques for robot processing, data storage, sensing, control and propulsion may be employed without loss of applicability of the present invention described herein.
Navigation
Navigation by an autonomous or semi-autonomous robot requires some form of spatial model of the robot's environment. Spatial models may be represented by bitmaps, object maps, landmark maps, and other forms of two- and three-dimensional digital representations. A spatial model of a warehouse facility, as shown in
Spatial models in a warehouse facility may also represent target locations such as a shelf or bin marked with a fiducial to which a robot may be directed to pick product or to perform some other task, or to a temporary holding location or to the location of a charging station. For example,
The spatial model most commonly used for robot navigation is a bitmap of an area or facility.
As depicted in
In some embodiments the supervisory system may comprise a central server performing supervision of a plurality of robots in a manufacturing warehouse or other facility, or the supervisory system may comprise a distributed supervisory system consisting of one or more servers operating within or without the facility either fully remotely or partially without loss of generality in the application of the methods and systems herein described. The supervisory system may include a server or servers having at least a computer processor and a memory for executing a supervisory system and may further include one or more transceivers for communicating information to one or more robots operating in the warehouse or other facility. Supervisory systems may be hosted on computer servers or may be hosted in the cloud and communicating with the local robots via a local transceiver configured to receive and transmit messages to and from the robots and the supervisory system over wired and/or wireless communications media including over the Internet.
One skilled in the art would recognize that robotic mapping for the purposes of the present invention could be performed using methods known in the art without loss of generality. Further discussion of methods for robotic mapping can be found in Sebastian Thrun, “Robotic Mapping: A Survey”, Carnegie-Mellon University, CMU-CS-02-111, February, 2002, which is incorporated herein by reference.
Obstacle Avoidance
To successfully navigate the goal path and arrive at the target product bin in the presence of dynamic obstacles, the robot must continually recalibrate its trajectory. At each recalibration, an instantaneous velocity is used to advance the robot one incremental movement along the goal path. For example, as shown in
Incremental movement of the robot on a trajectory colliding with other robots may be prevented by methods such as optimal reciprocal collision avoidance (ORCA). ORCA guarantees that one robot will not collide with another by assuming that each robot is also computing its next incremental movement using ORCA. In this manner, robots may navigate fully autonomously while ensuring an optimal collision-free path for each. A discussion of ORCA is described in Jur van den Berg, Stephen J. Guy, Ming Lin, and Dinesh Manocha, “Reciprocal n-body collision avoidance”, in Robotics Research: The 14th International Symposium ISRR, Cédric Pradalier, Roland Siegwart, and Gerhard Hirzinger (eds.), Springer Tracts in Advanced Robotics, Vol. 70, Springer-Verlag, May 2011, pp. 3-19, which is incorporated by reference herein.
Also known in the art of obstacle avoidance is the Dynamic Window Approach (DWA). DWA considers a set of Npossible instantaneous velocities for incremental movement of the robot along the goal path. DWA then scores the trajectory taken by the robot assuming incremental movement according to each instantaneous velocity taken over one or more time steps. Each trajectory is scored according to an objective function that takes into account non-robot obstacles and other factors. For example, each trajectory may be scored according to adherence to the goal path weighed against avoiding close proximity to obstacles. By further example, adjusting the behavior of the robot in the presence of humans working among the robots may be desired. A discussion of DWA is provided in D. Fox, W. Burgard, and S. Thrun, “The Dynamic Window Approach to Collision Avoidance,” in Robotics & Automation Magazine, IEEE, vol. 4, no. 1. (March 1997), pp. 23-33, which is incorporated by reference herein.
While DWA provides a flexible approach to controlling the behavior of the robot, it does so at the sacrifice of an optimal response when encountering ORCA-driven robots. Similarly, while ORCA provides a provably optimal and computationally efficient determination of the robot's next instantaneous velocity, ORCA does not account for non-robot obstacles and other factors important for optimizing the behavior of the robot as it navigates to the target location along the goal path.
ORCA/DWA
Beginning at step 802, robot system 600 receives an obstacle map via transceiver module 670, which may be stored in data storage 630. The obstacle map may be a SLAM map or other map, such as a cost map overlaid with obstacles. Alternatively, the obstacle map may be any spatial model capable of representing fixed obstacles within the robot environment. The obstacle map may be stored and subsequently retrieved from data storage 630 by data processor 620 or map processing module 646 or path planning module 642.
At step 806, robot system 600 receives a goal pose then generates, at step 808, the goal path to the target pose using path planning module 646. Path planning module 642 may generate the goal path from the current pose to the goal pose by a variety of techniques known to practitioners in the art including the A* and D* pathfinding algorithms. Alternatively, the robot may receive a goal path via transceiver module 670 or may retrieve a goal path from data storage 630. Having received the obstacle map and generated the goal path, robot system 600 then proceeds to move the robot incrementally along the goal path as follows.
At step 810, robot system 600 receives the current position and velocity of all robots in the area. Knowing the pose of all other robots relative to its pose, the robot can ignore robots far from its operating area. Alternatively, the robot may receive only the poses of robots operating in proximity to the robot without loss of generality of step 810. For example, referring again to
Additionally, in step 810, the robot receives its own pose. Preferably, the state of the robot, including its pose, may be determined by the robot system 600 using the odometry from the robot itself, the drive/train wheel encoders 664 and/or inertial sensor 668 or other sensor modules 660 or by processing modules 640 operating on other sensors or received information. The robot's pose may be determined by a fusion of the aforementioned inputs and/or by many-to-many multiresolution scan matching (M3RSM), adaptive monte-carlo localization (AMCL), geo-positioning satellite (GPS), fiducial information, or the robot's pose may be received from a supervisory system via transceiver 670.
At step 812, process 800 continues by performing ORCA/DWA, as particularly described below and in relation to
Beginning at step 852, according to one embodiment, the set of possible or “candidate” velocities is generated according to DWA. The candidate velocities may be expressed as a set of trajectories or “curvatures” uniquely determined by velocity vector (v, ω), where v is the forward or linear motion of the robot and ω is the rotational velocity of the robot. The set of candidate velocities may be a finite set of velocities sampled from the set of possible trajectories advancing the robot from the robot's current pose, or the set of trajectories may define a curvature defined by the discrete, instantaneous movements of the robot advancing by a constant velocity (v, ω) over one or more next time intervals.
Thus, the method at step 812 generates the set of N possible instantaneous velocities for the robot to adopt as its control for the next movement at the next time step. By iterating over each of the candidate velocities, the method estimates the end pose of the robot, as if the robot were to proceed with the candidate velocity over the next T seconds at increments oft seconds. Without loss of generality and for the purposes of illustrating the present invention, T may be 1.5 seconds and time increment t may be 0.1 second. Thus, by way of example only, if the candidate velocity is 0.25 meters/sec of linear velocity and 0.785 rad/sec of rotational velocity, the process estimates the pose at which the robot would arrive, applying as its control velocity vector (v, ω)=(0.25, 0.785), at each of the 0.1, 0.2, 0.3 . . . 1.5 second time steps. The set of poses at each time step make up the trajectory to be scored by the ORCA/DWA objective functions to be further described below.
Discretization by DWA in generating the candidate velocities necessarily selects less than the infinite number of trajectories that a robot could move in the next succession of time steps. The set of candidate velocities may be further reduced by removing from the set those velocities whose curvatures would intersect with an obstacle in the next time interval or the next n time intervals, assuming the robot continued with a selected candidate velocity. By pruning the set of candidate velocities to non-obstacle colliding, admissible velocities, the search space for determining a preferred velocity is reduced. The set of candidate velocities may be further reduced by retaining only the admissible velocities. Admissible velocities may include only those velocities that would allow a robot to stop before colliding with an obstacle. Admissible velocities may include only those velocities within the dynamic window of the robot or within a maximum safe speed allowed for the robot. The dynamic window of the robot may include only velocities that can be reached within the next time interval given the limited acceleration of the robot. Alternatively, the set of candidate velocities may be generated or pruned by other methods, such as model predictive control (MPC) or other constraints-based algorithm.
Returning to
G(v,ω)=α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω),
where G(v,ω) is the objective function, α, β, γ are weights; heading(v,ω) is a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle (its “clearance”); and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω). A detailed discussion of the evaluation of an objective function G(v,ω) under DWA is found in D. Fox et al., “The Dynamic Window Approach to Collision Avoidance,” as previously incorporated by reference above. One skilled in the art would understand variations on the objective function may be applied without loss of generality of the present invention.
Evaluating the objective function G(v,ω) for each of the candidate velocities determines a preferred velocity vpref by scoring each candidate velocity (v,ω) in the set of candidate velocities. Each candidate velocity (v,ω) is associated with a trajectory or curvature, as above. Preferably, objective function G(v,ω) is implemented as a cost function with weighted component functions representing one or more cost critics. In the cost critic embodiment, the preferred velocity vpref is the minimum G(v,ω) over the set of candidate velocities determined at step 856 of
Cost critics in some embodiments may include a dist(v,ω) cost function that uses inflated obstacles in the obstacle map (e.g. inflated by the radius or diameter of the robot) to ensure that all robots get safely past static obstacles. Cost critics may include an oscillation cost function assigning higher costs to changes in the magnitude or direction of rotation. Cost critics may further include, in a preferred embodiment, a path cost critic weighting the distance of the robot from the goal path and/or how far along the goal path the robot would be for a given candidate velocity, or how much the candidate velocity would cause the robot to radiate from the goal path.
Preferred velocity vpref determined by applying the DWA objective function G(v,ω) is thus the minimum cost candidate velocity upon evaluation of all of the DWA cost critics. One skilled in the art would appreciate that weights α, β, γ for the cost functions of G(v,ω) above may be set according to preferences for the behavior of the robot. Each of the component functions of the objective function G(v,ω) may have its own weight or no weighting (weight=1). Preferably, one or more cost critics have “veto power” to invalidate a candidate velocity by adding a relatively high penalty value to any candidate velocity cost function score that violates pre-determined criteria.
As explained above, the preferred trajectory tpref determined by DWA at step 858 of
Recalling that robot system 600 has already received, at step 810 of
ORCA, however, in at least one embodiment, requires preferred velocity vpref as an input and converts all velocities (v,ω) to holonomic velocities (x,y). That is, ORCA assumes that the robot can move in any direction (x,y). As one skilled in the art would understand, ORCA uses holonomic velocity vectors in its analysis, whereas vpref generated by DWA assumes non-holonomic robot control, as for differential drive robots. Thus, accommodation for some error must be made when converting the non-holonomic preferred velocity vpref to the holonomic vector (x, y) for use in generating ORCA VOs.
As would be understood by one skilled in the art, creating the VOs and then taking the union of these VOs forms a polygon in velocity space defined by the intersection of the half-planes of each of the VOs. A discussion of creating VOs and taking their union in the application of ORCA is found in Jur van den Berg et al., “Reciprocal n-body collision avoidance”, as previously incorporated by reference above. For example,
With the above in mind, the process continues in
ORCA/DWA=CDWA=αORCA*CORCA
CDWA is defined as:
CDWA=α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω),
where α, β, γ are weights; heading(v,ω) is a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle; and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω). CDWA and G(v,ω), as used above for determining vpref for ORCA VOs generation, may be implemented as the same function, with the same weights or as different functions with different weights or by combinations of other cost functions without loss of generality in application to the present invention.
CORCA is defined as follows:
CORCA=αv(vt-vpref)+penalty+αd*d(P,vt)
where αd and αv are weights; vt is a candidate velocity being evaluated; vpref is the preferred velocity; P is the polygon formed by the union of VOs; d (P, vt) is a measure of how much a candidate velocity violates the VOs; and penalty is a penalty cost imposed when a candidate velocity vt violates a VO.
In the evaluation of cost critic CORCA, all candidate velocities vt get a base cost αv (vt-vpref) as a measure of how much they differ from the preferred velocity vpref. For example, as depicted in
Further, in a preferred embodiment, an additional penalty may be imposed on vt based on the distance d (P, vt) from vt to the edge of polygon P. For example, shown in
Combining the ORCA and DWA cost critics and taking the minimum weighed cost:
min(CDWA+αORCA*CORCA)
returns the optimal candidate velocity from the set of candidate velocities. Returning to step 816 of
While the foregoing description of the invention enables one of ordinary skill to make and use what is considered presently to be the best mode thereof, those of ordinary skill will understand and appreciate the existence of variations, combinations, and equivalents of the specific embodiments and examples herein. The above-described embodiments of the present invention are intended to be examples only. Alterations, modifications and variations may be effected to the particular embodiments by those of skill in the art without departing from the scope of the invention, which is defined solely by the claims appended hereto.
It should be understood that the present invention may be implemented with software and/or hardware. Accordingly, aspects of the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment (including firmware, resident software, micro-code, etc.) or an embodiment combining software and hardware aspects that may all generally be referred to herein as a “circuit,” “module” or “system.” As will be appreciated by one skilled in the art, aspects of the invention may be embodied as a system, method or computer program product.
Aspects of the present invention are described with reference to flowcharts, illustrations and/or block diagrams of methods and apparatus (systems). The flowcharts and block diagrams may illustrate system architecture, functionality, or operations according to various embodiments of the invention. Each step in the flowchart may represent a module, which comprises one or more executable instructions for implementing the specified function(s). In some implementations, steps shown in succession may in fact be executed substantially concurrently. Steps may be performed by special purpose hardware-based systems that perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
Computer instructions for execution by a processor carrying out operations of the present invention may be written one or more programming languages, including object-oriented programming languages such as C#, C++, Python, or Java programming languages. Computer program instructions may be stored on a computer readable medium that can direct the robot system via the data processor to function in a particular manner, including executing instructions which implement the steps specified in a flowchart and/or system block diagram described herein. A computer readable storage medium may be any tangible medium that can contain, or store instructions for use by or in connection with the data processor. A computer readable medium may also include a propagated data signal with computer readable program code embodied therein.
The invention is therefore not limited by the above described embodiments and examples, embodiments, and applications within the scope and spirit of the invention claimed as follows.
| Number | Name | Date | Kind |
|---|---|---|---|
| 5006988 | Borenstein et al. | Apr 1991 | A |
| 8060254 | Myeong et al. | Nov 2011 | B2 |
| 8060306 | Puhalla et al. | Nov 2011 | B2 |
| 8209144 | Anguelov et al. | Jun 2012 | B1 |
| 8723872 | Beardsley et al. | May 2014 | B2 |
| 8793069 | Kuroda | Jul 2014 | B2 |
| 9067320 | Beardsley et al. | Jun 2015 | B2 |
| 9082233 | Beardsley et al. | Jul 2015 | B2 |
| 9145286 | Pfaff et al. | Sep 2015 | B2 |
| 9216745 | Beardsley et al. | Dec 2015 | B2 |
| 9286810 | Eade et al. | Mar 2016 | B2 |
| 9412173 | Maynooth et al. | Aug 2016 | B2 |
| 9592609 | LaFary et al. | Mar 2017 | B2 |
| 10108194 | Russell | Oct 2018 | B1 |
| 20110082585 | Sofman et al. | Apr 2011 | A1 |
| 20110304633 | Beardsley et al. | Dec 2011 | A1 |
| 20150284010 | Beardsley et al. | Oct 2015 | A1 |
| 20160129907 | Kim et al. | May 2016 | A1 |
| 20160363454 | Hatanaka et al. | Dec 2016 | A1 |
| 20170270466 | Kao | Sep 2017 | A1 |
| 20180307941 | Kohl | Oct 2018 | A1 |
| Number | Date | Country |
|---|---|---|
| 2045161 | Apr 2009 | EP |
| Entry |
|---|
| Fox D et al: “The Dynamic Window Approach to Collision Avoidance”, IEEE Robotics & Automation Magazine, IEEE, Service Center, Piscataway, NJ, US, vol. 4, No. 1, Mar. 31, 1997 (Mar. 31, 1997). |
| International Search Report and Written Opinion, dated Mar. 1, 2019, received in international patent application No. PCT/US2018/052165, 11 pages. |
| D. Fox, W. Burgard, and S. Thrun, “The Dynamic Window Approach to Collision Avoidance,” in Robotics & Automation Magazine, IEEE, vol. 4, No. 1. (Mar. 1997), pp. 23-33. |
| Berthold K. P. Horn, “Closed-form solution of absolute orientation using unit quaternions,” Journal of the Optical Society of America, 4(4), Apr. 1987, pp. 629-642. |
| Jur van den Berg, Stephen J. Guy, Ming Lin, and Dinesh Manocha. “Reciprocal n-body collision avoidance”, in Robotics Research: The 14th International Symposium ISRR, Cédric Pradalier, Roland Siegwart, and Gerhard Hirzinger (eds.), Springer Tracts in Advanced Robotics, vol. 70, Springer-Verlag, May 2011, pp. 3-19. |
| Frese, Udo & Wagner, René & Röfer, Thomas. (2010). A SLAM overview from a users perspective. KI. 24. 191-198. 10.1007/s13218-010-0040-4. |
| Edwin Olson, “M3RSM: Many-to-many multi-resolution scan matching”, Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Jun. 2015. |
| Edwin B. Olson, “Robust and Efficient Robotic Mapping”, PhD Dissertation, Carnegie-Mellon University, 2008. |
| Sebastian Thrun, “Robotic Mapping: A Survey”, Carnegie-Mellon University, CMU-CS-02-111, Feb. 2002. |
| Edwin B. Olson, “Real-Time Correlative Scan Matching”, in Proceedings of the 2009 IEEE International Conference on Robotics and Automation (ICRA'09), IEEE Press, Piscataway, NJ, USA, 2009, pp. 1233-1239. |
| Olson Edwin: “M3RSM: Many-to-many multi-resolution scan matching”, 2015 IEEE International Conference on Robotics and Automation (ICRA), IEEE, May 26, 2015 (May 26, 2015), pp. 5815-5821, XP033169227, DOI: 10.1109/ICRA.2015.7140013 [retrieved on Jun. 29, 2015]. |
| Suzuki Shintaro et al: “Initial self-localization based on shared information for multi-robot teleoperation system”, The SICE Annual Conference 2013, The Society of Instrument and Control Engineers—SICE, Sep. 14, 2013 (Sep. 14, 2013), pp. 2721-2726, XP032566351, [retrieved on Feb. 10, 2014]. |
| Jianping Xie et al: “A real-time robust global localization for autonomous mobile robots in large environments”, Control Automation Robotics&Vision (ICARCV), 2010 11th International Conference on, IEEE, Dec. 7, 2010 (Dec. 7, 2010). |
| Yuichiro Toda et al: “Intelligent planning based on multi-resolution map for simultaneous localization and mapping”, Robotic Intelligence in Informationally Structured Space (RIISS), 2011 IEEE Workshop on, IEEE, Apr. 11, 2011 (Apr. 11, 2011). |
| International Search Report and Written Opinion, dated Mar. 6, 2019, received in international patent application No. PCT/US2018/052190, 8 pages. |
| Number | Date | Country | |
|---|---|---|---|
| 20190094866 A1 | Mar 2019 | US |
