Method of determining a trajectory

Abstract

Disclosed is an embodiment of a method of determining a trajectory including defining columns relative to an area, assigning a column height to each of the columns corresponding to a height of a reference having a greatest height therein, defining surfaces relative to each column at the assigned column height, and defining a trajectory from a first one of the surfaces to a second one of the surfaces.

Description

METHOD OF DETERMINING A TRAJECTORYCROSS REFERENCE TO RELATED APPLICATION(S)

[0001] None.RESERVATION OF COPYRIGHTS

[0002] Portions of the disclosure of this document contain material that is subject to copyright protection. The copyright owner does not object to any reproduction of the document or disclosure as it appears in official records, but reserves all remaining rights under copyright.BACKGROUND

[0003] The present disclosure relates to vehicle navigation, more specifically estimating navigability of surfaces therefor.

[0004] Because the world is not flat and local terrains can be uneven, vehicles, particularly automated vehicles, not limited to robots, can fall over while driving on inclines / declines.Commonly occurring height discontinuities in the operating domain cause instability, not limited to unmapped height discontinuities that are not curbs wherein a sharp height discontinuity exists between a desired terrain and a neighboring terrain, such as sidewalk and grass.

[0005] A way of avoiding (1) falling over on inclines / declines and (2) risky height discontinuities within operating domain is by providing one or more of: a wider base, lower center of mass, larger tires, or more kinematic freedom (i.e. cars, quadraped robots, drones).

[0006] Some vehicles are not able to provide any of the foregoing solutions. So as not to limit the range of operating domains for robots that have limited design flexibility, optimizing path trajectory is an important option.

[0007] A lack of information about terrain can limit or preclude optimizing a trajectory with respect to terrain. Consequently, no discrimination is made between paths through smooth terrain and paths through rough terrain.

[0008] One way to overcome this limitation is to create a 2.5 dimensional (2.5D) representation of the terrain, that is, a surface representation with no floating voxels, like a topology map. This representation may involve a discretization of space from a bird’s eye view, where each cell contains the highest observed elevation in that region of terrain. A representation developed in this manner can intelligently inform the robot’s motion.AB672WO 1 / 19

[0009] The foregoing is intended to provide a contextual overview of some current issues and is not intended to be exhaustive.SUMMARY OF THE INVENTION

[0010] The invention is method of continuously estimating a 2.5D terrain map in the space around a robot based on sensors and the robot’s pose and motion, and determining optimal navigation trajectories for the robot.

[0011] An embodiment of a method of determining a trajectory configured according to principles of the invention includes defining columns relative to an area, assigning a column height to each of the columns corresponding to a height of a reference having a greatest height therein, defining surfaces relative to each column at the assigned column height, and defining a trajectory from a first one of the surfaces to a second one of the surfaces.

[0012] The invention provides improved elements and arrangements thereof, for the purposes described, which are inexpensive, dependable and effective in accomplishing intended purposes of the invention.

[0013] Other features and advantages of the invention will become apparent from the following description of the embodiments, which refers to the accompanying drawings.BRIEF DESCRIPTION OF THE DRAWINGS

[0014] The invention is described in detail below with reference to the following figures, throughout which similar reference characters denote corresponding features consistently, wherein:

[0015] Figs. 1 , 2A, 2B, 3, 17 and 18 are schematic representations of frames of reference for an embodiment of a method of determining a trajectory configured according to principles of the invention;

[0016] Figs. 4 and 9 are side view schematic representations of a robot traversing a surface according to a trajectory configured according to principles of the invention;

[0017] Figs. 5 and 7 are plan view graphical representations of sensor detections and segregation thereof configured according to principles of the invention;

[0018] Figs. 6 and 8 are side view graphical representations of sensor detections and segregation thereof configured according to principles of the invention;

[0019] Figs. 10 and 14 are plan view schematic representations of a robot traversing a surface according to a trajectory configured according to principles of the invention;AB672WO 2 / 19

[0020] Fig 11 is a top, side graphical representation of Fig. 10;

[0021] Fig. 12 is a graphical representation of columns based on the data of Fig. 11 configured according to principles of the invention;

[0022] Fig. 13 is a plan view of Fig. 12 including potential trajectories;

[0023] Fig 15 is a rear view schematic representation of Fig. 14; and

[0024] Fig. 16 is a graphical representation of columns based on Fig. 12 plus roll and pitch costs configured according to principles of the invention.DETAILED DESCRIPTION OF THE EMBODIMENTS

[0025] The examples shown in drawings are presented to demonstrate examples of the disclosure. The drawings are illustrative and non-limiting. In the drawings, for illustrative purposes, the size of some of the elements may be exaggerated and not drawn to a particular scale. Additionally, elements shown within the drawings that have the same numbers may be identical elements or may be similar elements, depending on the context.

[0026] Where the term "comprising" is used in the present description and claims, it does not exclude other elements or steps. Where an indefinite or definite article is used when referring to a singular noun, e.g., "a", "an", or "the", this includes a plural of that noun unless something otherwise is specifically stated. Hence, the term "comprising" should not be interpreted as being restricted to the items listed thereafter; it does not exclude other elements or steps, and so the scope of the expression "a device comprising items A and B" should not be limited to devices consisting only of components A and B. Furthermore, to the extent that the terms “includes”, “has”, “possesses”, and the like are used in the present description and claims, such terms are intended to be inclusive in a manner similar to the term “comprising,” as “comprising” is interpreted when employed as a transitional word in a claim.

[0027] Furthermore, the terms "first", "second", "third", and the like, whether used in the description or in the claims, are provided to distinguish between similar elements and not necessarily to describe a sequential or chronological order. It is to be understood that the terms so used are interchangeable under appropriate circumstances (unless clearly disclosed otherwise) and that the aspects of the disclosure described herein are capable of operation in other sequences and / or arrangements than are described or illustrated herein.

[0028] In the following description, numerous specific details are set forth to provide a thorough understanding of various aspects and arrangements. It will be recognized, however, that theAB672WO 3 / 19techniques described herein can be practiced without one or more of the specific details, or with other methods, components, materials, etc. In other instances, well known structures, materials, or operations may not be shown or described in detail to avoid obscuring certain aspects.

[0029] Reference throughout this specification to “an aspect,” “an arrangement,” “a configuration,” or “an example” indicates that a particular feature, structure, or characteristic is described. Thus, appearances of phrases such as “in one aspect,” “in one arrangement,” “in a configuration,” “in some examples,” or the like in various places throughout this specification do not necessarily each refer to the same aspect, feature, configuration, example, or arrangement. Furthermore, the particular features, structures, and / or characteristics described may be combined in any suitable manner.

[0030] To the extent used in the present disclosure and claims, the terms “component,” “system,” “platform,” “layer,” “selector,” “interface,” and the like are intended to refer to a computer-related entity or an entity related to an operational apparatus with one or more specific functionalities, wherein the entity may be either hardware, a combination of hardware and software, software, or software in execution. As an example, a component may be, but is not limited to being, a process running on a processor, a processor, an object, an executable, a thread of execution, a program, and / or a computer. By way of illustration and not limitation, both an application running on a server and the server itself can be a component. One or more components may reside within a process and / or thread of execution and a component may be localized on one computer and / or distributed between two or more computers. In addition, components may execute from various computer-readable media, device-readable storage devices, or machine-readable media having various data structures stored thereon. The components may communicate via local and / or remote processes such as in accordance with a signal having one or more data packets (e.g., data from one component interacting with another component in a local system, a distributed system, and / or across a network such as the Internet with other systems via the signal). As another example, a component can be an apparatus with specific functionality provided by mechanical parts operated by electric or electronic circuitry, which may be operated by a software or firmware application executed by a processor, wherein the processor can be internal or external to the apparatus and executes at least a part of the software or firmware application. As yet another example, a component can be an apparatus that provides specific functionality through electronic components without mechanical parts; the electronic components can include a processor therein to execute software or firmware that confers at least in part the functionality of the electronic components.AB672WO 4 / 19

[0031] To the extent used in the subject specification, terms such as “store,” “storage,” “data store,” data storage,” “database,” and the like refer to memory components, entities embodied in a memory, or components comprising a memory. It will be appreciated that the memory components described herein can be either volatile memory or nonvolatile memory, or can include both volatile and nonvolatile memory.

[0032] In addition, the term “or” is intended to mean an inclusive “or” rather than an exclusive “or.” That is, unless specified otherwise, or clear from context, “X employs A or B” is intended to mean any of the natural inclusive permutations. That is, if X employs A, X employs B, or X employs both A and B, then “X employs A or B” is satisfied under any of the foregoing instances. Moreover, articles “a” and “an” as used in the subject disclosure and claims should generally be construed to mean “one or more” unless specified otherwise or clear from context to be directed to a singular form.

[0033] The words “exemplary” and / or “demonstrative,” to the extent used herein, mean serving as an example, instance, or illustration. For the avoidance of doubt, the subject matter disclosed herein is not limited by disclosed examples. In addition, any aspect or design described herein as “exemplary” and / or “demonstrative” is not necessarily to be construed as preferred or advantageous over other aspects or designs, nor is it meant to preclude equivalent exemplary structures and techniques. Furthermore, to the extent that the terms “includes,” “has,” “contains,” and other similar words are used in either the detailed description or the claims, such terms are intended to be inclusive, in a manner similar to the term “comprising” as an open transition word, without precluding any additional or other elements.

[0034] As used herein, the term “infer” or “inference” refers generally to the process of reasoning about, or inferring states of, the system, environment, user, and / or intent from a set of observations as captured via events and / or data. Captured data and events can include user data, device data, environment data, data from sensors, application data, implicit data, explicit data, etc. Inference can be employed to identify a specific context or action or can generate a probability distribution over states of interest based on a consideration of data and events, for example.

[0035] The disclosed subject matter can be implemented as a method, apparatus, or article of manufacture using standard programming and / or engineering techniques to produce software, firmware, hardware, or any combination thereof to control a computer to implement the disclosed subject matter. The term "article of manufacture," to the extent used herein, is intended to encompass a computer program accessible from any computer-readable device, machine-readable device, computer-readable carrier, computer-readable media, or machine-AB672WO 5 / 19readable media. For example, computer-readable media can include, but are not limited to, a magnetic storage device, e.g., hard disk; floppy disk; magnetic strip(s); an optical disk (e.g., compact disk (CD), digital video disc (DVD), Blu-ray Disc (BD)); a smart card; a flash memory device (e.g., card, stick, key drive); a virtual device that emulates a storage device; and / or any combination of the above computer-readable media.

[0036] Generally, program modules include routines, programs, components, data structures, etc., that perform particular tasks or implement particular abstract data types. The illustrated aspects of the subject disclosure may be practiced in distributed computing environments where certain tasks are performed by remote processing devices that are linked through a communications network. In a distributed computing environment, program modules can be located in both local and remote memory storage devices.

[0037] Computing devices can include at least computer-readable storage media, machine-readable storage media, and / or communications media. Computer-readable storage media or machine-readable storage media can be any available storage media that can be accessed by the computer and includes both volatile and nonvolatile media, removable and non-removable media. By way of example, and not limitation, computer-readable storage media or machine-readable storage media can be implemented in connection with any method or technology for storage of information such as computer-readable or machine-readable instructions, program modules, structured data, or unstructured data.

[0038] Computer-readable storage media can include, but are not limited to, random access memory (RAM), read only memory (ROM), electrically erasable programmable read only memory (EEPROM), flash memory or other memory technology, compact disk read only memory (CD ROM), digital versatile disk (DVD), Blu-ray disc (BD) or other optical disk storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, solid state drives or other solid state storage devices, or other tangible and / or non-transitory media that can be used to store desired information. In this regard, the terms “tangible” or “non-transitory” herein as applied to storage, memory, or computer-readable media, are to be understood to exclude only propagating transitory signals per se as modifiers, and do not exclude any standard storage, memory, or computer-readable media that are not only propagating transitory signals per se.

[0039] Computer-readable storage media can be accessed by one or more local or remote computing devices, e.g., via access requests, queries, or other data retrieval protocols, for a variety of operations with respect to the information stored by the medium.AB672WO 6 / 19

[0040] A system bus, as may be used herein, can be any of several types of bus structure that can further interconnect to a memory bus (with or without a memory controller), a peripheral bus, and a local bus using any of a variety of commercially available bus architectures. A database, as may be used herein, can include basic input / output system (BIOS) that can be stored in a non-volatile memory such as ROM, EPROM, or EEPROM, with BIOS containing the basic routines that help to transfer information between elements within a computer, such as during startup. RAM can also include a high-speed RAM such as static RAM for caching data.

[0041] As used herein, a computer can operate in a networked environment using logical connections via wired and / or wireless communications to one or more remote computers. The remote computer(s) can be a workstation, server, router, personal computer, portable computer, microprocessor-based entertainment appliance, peer device, or other common network node. Logical connections depicted herein may include wired / wireless connectivity to a local area network (LAN) and / or larger networks, e.g., a wide area network (WAN). Such LAN and WAN networking environments are commonplace in offices and companies, and facilitate enterprisewide computer networks, such as intranets, any of which can connect to a global communications network, e.g., the Internet.

[0042] When used in a LAN networking environment, a computer can be connected to the LAN through a wired and / or wireless communication network interface or adapter. The adapter can facilitate wired or wireless communication to the LAN, which can also include a wireless access point (AP) disposed thereon for communicating with the adapter in a wireless mode.

[0043] When used in a WAN networking environment, a computer can include a modem or can be connected to a communications server on the WAN via other means for establishing communications over the WAN, such as by way of the Internet. The modem, which can be internal or external, and a wired or wireless device, can be connected to a system bus via an input device interface. In a networked environment, program modules depicted herein relative to a computer or portions thereof can be stored in a remote memory / storage device.

[0044] When used in either a LAN or WAN networking environment, a computer can access cloud storage systems or other network-based storage systems in addition to, or in place of, external storage devices. Generally, a connection between a computer and a cloud storage system can be established over a LAN or a WAN, e.g., via an adapter or a modem, respectively. Upon connecting a computer to an associated cloud storage system, an external storage interface can, with the aid of the adapter and / or modem, manage storage provided by the cloud storage system as it would other types of external storage. For instance, the external storageAB672WO 7 / 19interface can be configured to provide access to cloud storage sources as if those sources were physically connected to the computer.

[0045] As employed in the subject specification, the term “processor” can refer to substantially any computing processing unit or device comprising, but not limited to comprising, single-core processors; single-core processors with software multithread execution capability; multi-core processors; multi-core processors with software multithread execution capability; multi-core processors with hardware multithread technology; vector processors; pipeline processors; parallel platforms; and parallel platforms with distributed shared memory. Additionally, a processor can refer to an integrated circuit, an application specific integrated circuit (ASIC), a digital signal processor (DSP), a field programmable gate array (FPGA), a programmable logic controller (PLC), a complex programmable logic device (CPLD), a state machine, a discrete gate or transistor logic, discrete hardware components, or any combination thereof designed to perform the functions described herein. Processors can exploit nano-scale architectures such as, but not limited to, molecular and quantum-dot based transistors, switches, and gates, in order to optimize space usage or enhance performance of user equipment. A processor may also be implemented as a combination of computing processing units. For example, a processor may be implemented as one or more processors together, tightly coupled, loosely coupled, or remotely located from each other. Multiple processing chips or multiple devices may share the performance of one or more functions described herein, and similarly, storage may be effected across a plurality of devices. A processor may be implemented to reside in a cloud-based network such as, e.g., the Internet.

[0046] The actions of a method or algorithm described in connection with the arrangements disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. A software module may reside in RAM memory, flash memory, ROM memory, EPROM memory, EEPROM memory, registers, hard disk, a removable disk, a CD-ROM, or any other known form of storage medium. A storage medium may be coupled to the processor such that the processor can read information from, and write information to, the storage medium. In the alternative, the storage medium may be integral to the processor. The processor and the storage medium may reside in an ASIC. The ASIC may reside in functional equipment such as, e.g., a computer, a robot, a user terminal, a mobile telephone or tablet, a car, or an IP camera. In the alternative, the processor and the storage medium may reside as discrete components in such functional equipment. Additionally or alternatively, at least one of the processor and / or the storage medium may reside in a cloudbased network such as, e.g., the Internet.AB672WO 8 / 19

[0047] Configurations of the present teachings are directed to computer systems for accomplishing the methods discussed in the description herein, and to computer readable media containing programs for accomplishing these methods. The raw data and results can be stored for future retrieval and processing, printed, displayed, transferred to another computer, and / or transferred elsewhere. Communications links can be wired or wireless, for example, using cellular communication systems, military communications systems, and satellite communications systems. Parts of the system can operate on a computer having a variable number of CPUs. Other alternative computer platforms can be used.

[0048] The present configuration is also directed to software / firmware / hardware for accomplishing the methods discussed herein, and computer readable media storing software for accomplishing these methods. The various modules described herein can be accomplished on the same CPU, or can be accomplished on different CPUs. In compliance with the statute, the present configuration has been described in language more or less specific as to structural and methodical features. It is to be understood, however, that the present configuration is not limited to the specific features shown and described, since the means herein disclosed comprise nonexclusive forms of putting the present configuration into effect.

[0049] Methods can be, in whole or in part, implemented electronically. Signals representing actions taken by elements of the system and other disclosed configurations can travel over at least one live communications network. Control and data information can be electronically executed and stored on at least one computer-readable medium. The system can be implemented to execute on at least one computer node in at least one live communications network. Common forms of at least one computer-readable medium can include, for example, but not be limited to, a floppy disk, a flexible disk, a hard disk, magnetic tape, or any other magnetic medium, a compact disk read only memory or any other optical medium, punched cards, paper tape, or any other physical medium with patterns of holes, a random access memory, a programmable read only memory, and erasable programmable read only memory (EPROM), a Flash EPROM, or any other memory chip or cartridge, or any other medium from which a computer can read. Further, the at least one computer readable medium can contain graphs in any form, subject to appropriate licenses where necessary, including, but not limited to, Graphic Interchange Format (GIF), Joint Photographic Experts Group (JPEG), Portable Network Graphics (PNG), Scalable Vector Graphics (SVG), and Tagged Image File Format (TIFF).

[0050] Various arrangements are described herein. For simplicity of explanation, the methods or algorithms are depicted and described as a series of steps or actions. It is to be understoodAB672WO 9 / 19and appreciated that the various arrangements are not limited by the actions illustrated and / or by the order of actions. For example, actions can occur in various orders and / or concurrently, and with other actions not presented or described herein. Furthermore, not all illustrated actions may be required to implement the methods. In addition, the methods could alternatively be represented as a series of interrelated states via a state diagram or events. Additionally, the methods described hereafter are capable of being stored on an article of manufacture, as defined herein, to facilitate transporting and transferring such methodologies to computers.

[0051] The invention is method of continuously estimating a 2.5D terrain map in the space around a robot based on sensors and the robot’s pose and motion, and determining optimal navigation trajectories for the robot.

[0052] Referring to Fig. 1, an embodiment of a method 200 configured according to principles of the invention may be understood in terms of some reference frames. A first reference frame 10 is orthogonal to a gravity vector 15 of the earth 20. A ramp feature 25 exists relative to frame 10.

[0053] Referring also to Fig. 2A, an enlarged view of ramp feature 25 represents a robot 30 (Fig. 3) with a representation 35, in this example in the form of a dot. Fig. 2B shows a plan view of dot 35 on ramp feature 25 wherein shading corresponding to height increases from lower values on the left-hand side thereof to higher values on the right-hand side thereof. Figs. 2A and 2B include a map coordinate reference frame 40 wherein the z-axis is aligned with gravity vector 15 (Fig 1), the x-axis is aligned with a direction from prior mapping data collections, e.g. always due East, and the y-axis is orthogonal to the x-axis and z-axis following the right-hand rule. The z value of a point represents the elevation above the xy plane of the map frame, a plane parallel to sea level relative to which a terrain is estimated.

[0054] Referring to Fig. 3, a robot coordinate reference frame 45 is fixed relative to part of the body or virtual body of robot 30. The position of robot 30, represented by frame 45, can be represented with six degrees of freedom (6DOF) with respect to map frame 40. The X-Y plane of frame 45 is defined by the four contact points on surface 55 by the wheels 50 of robot 30.

[0055] Robot 30 includes a localization system (not shown) that is capable of assessing a current pose of robot 30, the position and orientation of reference frame 45 with respect to map frame 40.

[0056] Referring also to Fig. 17, robot 30 includes a sensor 65 for longer-range detection of an environment of robot 30, not limited to a LiDAR sensor. Sensor 65 has a first sensor coordinate reference frame 60 fixed relative to part of the actual or virtual first sensor 65 fixed relative to part of the body of robot 30. Frame 60 defines a context for measurements from first sensor 65.AB672WO 10 / 19

[0057] When robot 35 boots up, it defines a grid L centered on robot 35 not limited to 12.6 meters per side with 5cm resolution and having an origin LO and corresponding x-y axes. The origin LO corresponds to the position of robot 35 with respect to the map frame 40. The cells in grid L are always aligned with the xy axes of map frame 40. The grid does not orient with the robot as it rotates. As sensor 65 sweeps around robot 35 it detects environmental features that are transcribed into points 85, defining a point cloud, relative to frame 60. This point cloud is a collection of points (x,y,z) relative to frame 60, with a single associated timestamp. Each point (x,y,z) relative to frame 60 is then transformed into a point relative to frame 45 using the position and orientation of the sensor relative to frame 45 (constant throughout time). Each point (x,y,z) relative to frame 45 is then transformed to a point (x,y,z) relative to map frame 40, using the position and orientation of the robot (frame 45) relative to map frame 40 from the localization system, at the corresponding moment in time. These points relative to map frame 40 are now placed on grid L.

[0058] Referring also to Fig. 18, as robot 35 moves, grid L moves with robot 35 from Lt0to Ltirelative to the origin of map frame 40, at a resolution not limited to 5cm, effectively “snapping” to the new position of the robot after every axis-aligned 5cm displacement. Sensor 65 sweeps around robot 35 again and detects known and unknown environmental features. New measurements of points 85 of known features are transformed to be relative to map frame 40. The corresponding cell in the grid Ln would then be updated. Heights (z-coordinates) of points 85 associated with known features (cells that already have a value) are updated based on the more recent detections of the same points according to Formula 1 below where a is a weighting constant such that 0 < a < 1.Formula 1 Z=(a *ZM) + [(1 - a) * (Zt)]

[0059] Consequently, features that are within 12.6 meters of the robot are retained in the original discrete space of grid L, just shifted to different rows / columns as the robot moves. .

[0060] Robot 30 includes a sensor 75 for shorter-range detection of the environment, not limited to RGB and / or stereo cameras. Second sensor 75 has a second sensor coordinate reference frame 70 fixed relative to part of the actual or virtual second sensor 75 fixed to part of the body of the robot 30. Frame 70 defines a context of measurements from second sensor 75.

[0061] Referring to Fig. 4, as robot 30 operates, sensor 65 projects a cone-shaped beam of energy 80 that is reflected back for detection and analysis by sensor 65. For reasons known to those of ordinary skill in the art, the raw measurements or detected points 85 define a point cloud that correspond to the surface or terrain 90 measured but have a positional uncertainty asAB672WO 11 / 19discussed below. The detections are not limited to the terrain, but also include what is on the terrain.

[0062] Fig. 5 is a plan view of robot 30, here represented by a symbol 35 having the form of a triangle or arrow, and points 85 corresponding to terrain 90 in Fig. 4. An embodiment of method 200 includes a step 202 of discretizing or defining columns 95, in this case having a square shape, where points 85 exist relative to terrain 90. Alternative embodiments of the invention include columns of other shapes and / or within curved planes or space.

[0063] Fig. 6, a view in direction 100 of Fig. 5, shows points 85 within their respective columns 95. One embodiment of method 200 includes, for each column, a step 204 of evaluating the points therein and a step 206 of determining a maximum height thereof. An embodiment of method 200 includes, for each column, prior to any evaluating, a step 208 of voxelizing the points therein and a step 210 of determining the maximum height of the voxels. Evaluating voxels rather than points may be more computationally efficient.

[0064] Referring to Fig. 7, the height information gained from either embodiment above enables development of a quasi-topographical map. To this end, an embodiment of method 200 includes a step 212 of generating a height map 105 based on height data from either of steps 206 or 210. Each column 95 has a uniform color that corresponds to the uniform height that is assigned to the column based on the maximum height analysis of step 206 or 210.

[0065] Referring to Figs. 8 and 9, a view in direction 105 of Fig. 7, the uniform heights 107 assigned to each column 95 are more readily apparent. Fig. 8 also reveals missing data 110 of the height data. As shown in Fig. 9, missing data 110 are a consequence a section 115 of terrain 90 existing in an area that sensor 60 cannot readily perceive from it’s momentary position. These shadowed areas tend to present at a distance from robot 30 so are not necessarily representative of an immediate threat. As robot 300 approaches section 115, sensor 60 on robot 30 gains an increasingly better vantage point for resolving uncertainties of section 115. Should robot 30 and sensor 60 fail to observe and resolve section 115 within a distance 120 from robot 30 then the invention includes preventing robot 30 from continuing in a path that would lead in an unobserved area. The length of distance 120 depends on view field of sensor 70 or whatever sensor robot 30 employs for close-up detections.

[0066] One embodiment of the invention provides for limiting the extent or overall size of a shadow section 115 by assuming a margin extending by an amount 130 from observed terrain 90 into unobserved terrain of section 115 at a height that corresponds to a height of an adjacent portion of terrain 90. Amounts 130a and 130b do not need to be equal. For example, if point 135 having a height hi is the last observed point before section 115, then margin 130a isAB672WO 12 / 19assigned height hi . While margins 130 are shown to be level or orthogonal to a gravity normal, margins 130 may be constructed to align with: an angle incline that robot 30 measures or calculates based on historical data; a line or plane defined by points proximate to section 115; or as desired for navigational purposes.

[0067] Referring to Fig. 10, an embodiment of the invention aids in navigation by associating costs with the height estimates and evaluating potential trajectories in terms of these costs. Terrain height estimations impact many considerations of trajectory planning, not limited to the position of a robot on the terrain, the orientation of the robot due to the height of the terrain at that position (roll and pitch), and the speed of the robot. As such, it can be difficult to visualize the cost of a planned robot pose given a terrain estimation (height map), due to its high dimensionality. For example, robot 35 intended for reaching a goal G may have to evaluate a variety of trajectories T that encounter or have to manage disturbances between a current pose P and goal G. The disturbances may include an obstacle O and / or a surface disturbance, such as a speed bump S. The disturbances may constitute absolute blocks, such as insurmountable walls of object O, or localized areas that may be navigable but impact stability of robot 35 depending on its velocity and / or path relative thereto.

[0068] Resource limitations are an important consideration. Having a map that defines cost for a given height map, for each possible speed the robot could be going, and each possible orientation of the robot can be computationally much more expensive than evaluating each of the sampled trajectories with respect to these costs, depending on how many trajectories are evaluated. Therefore, an embodiment of the invention evaluates trajectories with respect to cost, rather than defining a cost map that takes into consideration all variables.

[0069] Referring to Figs. 11 and 12, sensor 60 (Fig. 2) detects object O and speedbump S and returns the raw data points 85 as shown on Fig 11. As explained above, the raw data points 85 are convertible into a height map composed of columns that each assume the height of the voxel having the greatest height therein, as shown in Fig. 12. Data points 85 associated with object O are converted into voxels that define columns Co, and data points 85 associated with speedbump S are converted into voxels that define columns Cs. By placing a hypothetical robot onto a cell and determining where the contact points of it’s wheels would touch the terrain, an estimate of the stability of the robot in that position can be determined. In this case, heights of columns Co are high, whereas the adjacent columns (red) are not, such that the robot having two wheels on columns Co and two wheels on columns Cswould be a more unstable position than all four wheels on columns Csor all four wheels on columns Co., The relative heights of theAB672WO 13 / 19four contact points are what is important here, not the absolute heights or the reference plane, so long as it is orthogonal to the gravity vector, which physically influences stability.

[0070] Referring to Fig. 13, the cost of a potential trajectory T is evaluated as the sum of the cost of the individual poses P1-Pn of the trajectory. Trajectory T 1 has robot 35 transitioning from columns Csto columns Co, resulting in a high cost. Accordingly, trajectory T1 would have high cost and would be discarded.

[0071] Trajectory T3 has robot 35 in positions where it would experience less pitch or roll, therefore be more stable. Accordingly, trajectory T3 would have a moderate cost and remain in contention.

[0072] Referring also to Fig. 14, while trajectory T2 theoretically has robot 35 generally avoiding positions of instability, as shown in Fig 13, factoring in the width of robot 35 reveals a slight overlap with the right side of robot 35 and speedbump S, as shown in Fig 14. Accordingly, trajectory T2 would have a cost as described below.

[0073] Referring also to Fig. 15, with some robots, such as quadruped or even wheeled robots that employ active suspension, moderate disturbances in surface height, which a speedbump S would represent, may be accommodated by individually, actively / passively positioning the ground-engaging element to accommodate relative height differences to maintain stability of the robot. However, a robot 30 with passive if any suspension, as contemplated by the present invention, must instead choose a trajectory that will not cause unrecoverable instability. Such selection must consider, not only a location of a center of gravity CG, centered at frame 45, relative to a zone of stability ZS, without which robot 35 would fall over, but also potential roll (rotation about the x-axis of frame 45, as shown in Fig. 14) and / or pitch (rotation about the y-axis of frame 45, as shown in Fig. 14) that could be induced by localized surface height changes, such as a pothole or stone. Depending on the robot’s geometry, roll potential may be assigned a greater cost than a pitch potential. In such cases, in addition to avoiding obstacle O, a trajectory that limits roll and maintains pitch within a threshold, would have the least cost.

[0074] Comparing Figs. 12 and 16, a component of each of pitch and roll is speed. Roll and pitch scenarios should be accorded higher weight as speed increases because encountering bumps and / or potholes can subject the robot to more dangerous situations the faster it goes. Accordingly, the invention treats or renders height maps similar to as described above but with costs assessed as a function of speed. For example, impediments rendered as columns Co and Cs in Fig. 12 for a robot 35 traveling a first speed, would be rendered as columns Cos and Css in Fig. 16 for a robot 35 traveling a second speed that is greater than the first speed, the increased speed imposing greater costs for potential roll and pitch. This encourages the robot toAB672WO 14 / 19choose potential trajectories having lower speeds when it may encounter uneven terrain, and maximize stability.

[0075] While the principles of the invention have been described herein, the foregoing description is only an example and not a limitation on the scope of the invention. Other embodiments are contemplated within the scope of the present invention in addition to the exemplary embodiments shown and described herein. Modifications and substitutions by one of ordinary skill in the art are within the scope of the present invention. The invention is not limited to the particular embodiments described and depicted herein, rather only to the following claims.AB672WO 15 / 19

Claims

1. CLAIMSWE CLAIM:

1. Method of determining a trajectory comprising:(a) defining columns relative to an area;(b) assigning a column height to each of the columns corresponding to a height of a reference having a greatest height therein;(c) defining surfaces relative to each column at the assigned column height; and (d) defining a trajectory from a first one of the surfaces to a second one of the surfaces.

2. Method of claim 1 further comprising detecting an environment and generating point cloud data pertinent to the area.

3. Method of claim 1 wherein the reference is a datum of point cloud data pertinent to the area and / or a voxel defined relative to the datum.

4. Method of claim 1 wherein the first one and the second one are adjacent.

5. Method of claim 1 wherein the first one and the second one define a height difference within a parameter.

6. Method of claim 1 further comprising assigning a column without a reference an estimated height comprising an average of column heights of adjacent columns.

7. Method of claim 6 wherein the adjacent columns have references.AB672WO 16 / 198. Method of claim 1 further comprising:determining the height of the reference based on a first measurement; determining a second height of the reference based on a second measurement; updating the height of the reference with an estimate calculated according to:estimate = (a * second height) + [(1 - a) * (height)] wherein:a is a weighting constant; and0 < a < 1.

9. Method of claim 1 further comprising assigning a cost to the trajectory based on the first surface and the second surface.

10. Method of claim 9, wherein the trajectory comprises multiple trajectories, each having a respective cost, further comprising selecting one of the trajectories having a lowest cost.

11. Method of claim 1 further comprising defining:a second trajectory from the second one to a third one of the surfaces; anda path comprising the trajectory and the second trajectory.

12. Method of claim 11 further comprising:assigning a first cost to the trajectory;assigning a second cost to the second trajectory; anddefining a path cost based on the first cost and the second cost.

13. Method of claim 12, wherein the path comprises multiple paths, each having a respective path cost, further comprising selecting one of the paths having a lowest path cost.

14. Method of claim 1 further comprising estimating a first pose in the one of the surfaces.

15. Method of claim 14 further comprising estimating a second pose based on the first pose and a trajectory or a linear velocity and an angular velocity.

16. Method of claim 15 further comprising, relative to the second pose, returning to step a.AB672WO 17 / 1917. Method of claim 16 before said returning, further comprising recording linear and / or angular velocity commands, wherein said defining a trajectory comprises the linear and / or angular velocity commands.

18. Method of claim 1 further comprising generating an instruction based on the trajectory configured for controlling a motion of the vehicle.

19. System for moving a vehicle comprising a processor configured for performing the method of claim 1.

20. Apparatus for moving a vehicle comprising a non-transient, computer-readable medium configured for storing instructions configured for the method of claim 1.AB672WO 18 / 19

Images