System and method for providing a comprehensive trajectory planner for a person-following vehicle

A system and method for providing a comprehensive trajectory planner for a person-following vehicle that includes receiving image data and LiDAR data associated with a surrounding environment of a vehicle. The system and method also include analyzing the image data and detecting the person to be followed that is within an image and analyzing the LiDAR data and detecting an obstacle that is located within a predetermined distance from the vehicle. The system and method further include executing a trajectory planning algorithm based on fused data associated with the detected person and the detected obstacle.

BACKGROUND

Person following has been researched on a wide variety of robotic platforms, including wheel chairs, legged robots, and skid-steer platforms. In many cases, person following has been researched assuming a static environment and/or unobstructed pathway environments (e.g., that include unobstructed sidewalks, hallways, gridded paths, etc.) to allow robotic platforms to uncritically follow a person. Techniques that have been developed based on such research with respect to person following in such static and/or unobstructed pathway environments have been found not to be practical in real-world applications that may require a determination of open spaces that may be navigated to avoid any overlap with potential obstacles. For example, in some cases, a person may fit through a tight gap between two obstacles which may not be easily navigated by a robotic platform. Accordingly, such techniques may result in a miscalculation of available open space based on the following of a person which may inhibit the unobstructed movement of the robotic platform.

BRIEF DESCRIPTION

According to one aspect, a computer-implemented method for providing a comprehensive trajectory planner for a person-following vehicle that includes receiving image data and LiDAR data associated with a surrounding environment of a vehicle. The computer-implemented method also includes analyzing the image data and detecting the person to be followed that is within an image and analyzing the LiDAR data and detecting an obstacle that is located within a predetermined distance from the vehicle. The computer-implemented method further includes executing a trajectory planning algorithm based on fused data associated with the detected person and the detected obstacle. The trajectory planning algorithm utilizes nonlinear model predictive control to enable the vehicle to follow the person within the surrounding environment of the vehicle.

According to another aspect, a system for providing a comprehensive trajectory planner for a person-following vehicle that includes a memory storing instructions when executed by a processor cause the processor to receive image data and LiDAR data associated with a surrounding environment of a vehicle. The instructions also cause the processor to analyze the image data and detecting the person to be followed that is within an image and analyze the LiDAR data and detecting an obstacle that is located within a predetermined distance from the vehicle. The instructions further cause the processor to execute a trajectory planning algorithm based on fused data associated with the detected person and the detected obstacle. The trajectory planning algorithm utilizes nonlinear model predictive control to enable the vehicle to follow the person within the surrounding environment of the vehicle.

According to yet another aspect, a non-transitory computer readable storage medium storing instructions that when executed by a computer, which includes a processor perform a method that includes receiving image data and LiDAR data associated with a surrounding environment of a vehicle. The computer-implemented method also includes analyzing the image data and detecting a person to be followed that is within an image and analyzing the LiDAR data and detecting an obstacle that is located within a predetermined distance from the vehicle. The computer-implemented method further includes executing a trajectory planning algorithm based on fused data associated with the detected person and the detected obstacle. The trajectory planning algorithm utilizes nonlinear model predictive control to enable the vehicle to follow the person within the surrounding environment of the vehicle.

DETAILED DESCRIPTION

The following includes definitions of selected terms employed herein. The definitions include various examples and/or forms of components that fall within the scope of a term and that may be used for implementation. The examples are not intended to be limiting.

A “bus”, as used herein, refers to an interconnected architecture that is operably connected to other computer components inside a computer or between computers. The bus may transfer data between the computer components. The bus may be a memory bus, a memory controller, a peripheral bus, an external bus, a crossbar switch, and/or a local bus, among others. The bus may also be a vehicle bus that interconnects components inside a vehicle using protocols such as Controller Area network (CAN), Local Interconnect Network (LIN), among others.

A “computer-readable medium”, as used herein, refers to a medium that provides signals, instructions and/or data. A computer-readable medium may take forms, including, but not limited to, non-volatile media and volatile media. Non-volatile media may include, for example, optical or magnetic disks, and so on. Volatile media may include, for example, semiconductor memories, dynamic memory, and so on. Common forms of a computer-readable medium include, but are not limited to, a floppy disk, a flexible disk, a hard disk, a magnetic tape, other magnetic medium, other optical medium, a RAM (random access memory), a ROM (read only memory), and other media from which a computer, a processor or other electronic device may read.

A “data store”, as used herein can be, for example, a magnetic disk drive, a solid state disk drive, a floppy disk drive, a tape drive, a Zip drive, a flash memory card, and/or a memory stick. Furthermore, the disk can be a CD-ROM (compact disk ROM), a CD recordable drive (CD-R drive), a CD rewritable drive (CD-RW drive), and/or a digital video ROM drive (DVD ROM). The disk can store an operating system that controls or allocates resources of a computing device. The data store can also refer to a database, for example, a table, a set of tables, a set of data stores (e.g., a disk, a memory, a table, a file, a list, a queue, a heap, a register) and methods for accessing and/or manipulating those data in those tables and data stores. The data store can reside in one logical and/or physical entity and/or may be distributed between two or more logical and/or physical entities.

A “memory”, as used herein can include volatile memory and/or non-volatile memory. Non-volatile memory can include, for example, ROM (read only memory), PROM (programmable read only memory), EPROM (erasable PROM), and EEPROM (electrically erasable PROM). Volatile memory can include, for example, RAM (random access memory), synchronous RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), and direct RAM bus RAM (DRRAM). The memory can store an operating system that controls or allocates resources of a computing device.

An “operable connection”, or a connection by which entities are “operably connected”, is one in which signals, physical communications, and/or logical communications can be sent and/or received. An operable connection can include a physical interface, a data interface and/or an electrical interface.

A “processor”, as used herein, processes signals and performs general computing and arithmetic functions. Signals processed by the processor can include digital signals, data signals, computer instructions, processor instructions, messages, a bit, a bit stream, or other means that may be received, transmitted and/or detected. Generally, the processor may be a variety of various processors including multiple single and multicore processors and co-processors and other multiple single and multicore processor and co-processor architectures. The processor may include various modules to execute various functions.

A “portable device”, as used herein, is a computing device typically having a display screen with user input (e.g., touch, keyboard) and a processor for computing. Portable devices include, but are not limited to, key fobs, handheld devices, mobile devices, smart phones, laptops, tablets and e-readers.

An “electric vehicle” (EV), as used herein, refers to any moving vehicle that is capable of carrying one or more human occupants and is powered entirely or partially by one or more electric motors powered by an electric battery. The EV may include battery electric vehicles (BEVs), plug-in hybrid electric vehicles (PHEVs) and extended range electric vehicles (EREVs). The term “vehicle” includes, but is not limited to: cars, trucks, vans, minivans, SUVs, motorcycles, scooters, boats, personal watercraft, and aircraft.

A “value” and “level”, as used herein may include, but is not limited to, a numerical or other kind of value or level such as a percentage, a non-numerical value, a discrete state, a discrete value, a continuous value, among others. The term “value of X” or “level of X” as used throughout this detailed description and in the claims refers to any numerical or other kind of value for distinguishing between two or more states of X. For example, in some cases, the value or level of X may be given as a percentage between 0% and 100%. In other cases, the value or level of X could be a value in the range between 1 and 10. In still other cases, the value or level of X may not be a numerical value, but could be associated with a given discrete state, such as “not X”, “slightly x”, “x”, “very x” and “extremely x”.

Referring now to the drawings, wherein the showings are for purposes of illustrating one or more exemplary embodiments and not for purposes of limiting same,FIG.1is a high-level schematic view of an illustrative system100for providing a comprehensive trajectory planner for a person-following vehicle102according to an exemplary embodiment of the present disclosure. The components of the system100, as well as the components of other systems and architectures discussed herein, may be combined, omitted or organized into different architectures for various embodiments.

In an exemplary embodiment ofFIG.1, the vehicle102may be configured to follow (e.g., within a predetermined distance) a particular individual as a goal104(person) that is traveling (e.g., walking, running, etc.) within a surrounding environment of the vehicle102. In an exemplary embodiment, the vehicle102may be configured as an all-terrain vehicle (ATV) as shown inFIG.1that may be configured to travel in paved and unpaved (e.g., off-road) environments and may be configured to carry heavy payloads (e.g., objects, persons). However, it is appreciated that the vehicle102may be configured in variety of various formats that may include, but may not be limited to, a legged robot, a wheeled robot, a skid-steer platform, a fork lift, a wagon, a shopping cart, a suit case, a personal transportation vehicle, a stroller, a baby/child carrier, a scooter, and/or additional types of occupant/object transportation devices.

In some embodiments, the vehicle102may be configured to operate in a manual mode by the person104, such that the person104may move the vehicle102manually (e.g., by pushing, pulling, and/or manually driving the vehicle102). The vehicle102may additionally be configured to be operated in a semi-automatic mode by the person104, such that a motor/engine (not shown) of the vehicle102may provide a certain amount of motive power to assist in moving the vehicle102. The vehicle102may additionally be operated within an autonomous mode. Within the autonomous mode, the vehicle102may operably be controlled to follow the person104that is located within a predetermined distance of the vehicle102in a fully autonomous or semi-autonomous manner within the surrounding environment of the vehicle102based on execution of a comprehensive trajectory planner vehicle control application106(vehicle control application). In one or more embodiments, the vehicle102may be configured as a front-wheel steered vehicle. In additional embodiments, the vehicle102may be configured as an all-wheel steered vehicle and/or a rear wheel steered vehicle.

In an exemplary embodiment, an externally hosted server infrastructure (external server)108and/or an electronic control unit (ECU)110of the vehicle102may be configured to execute the vehicle control application106. As discussed in more detail below, the vehicle control application106may be configured to execute a trajectory planning algorithm to enable the vehicle102to follow the person104. The trajectory planning algorithm may be configured to enable the vehicle102to maneuver in various manners. In one configuration, the trajectory planning algorithm executed by the vehicle control application106may be configured for the front-wheel steered vehicle102to allow the vehicle102to follow the person104. In alternate configurations, the trajectory planning algorithm may alternatively be configured for all-wheel steered vehicles or rear wheel steered vehicles.

In one embodiment, the trajectory planning algorithm executed by the vehicle control application106may be configured to enable the vehicle102to follow the person104while avoiding any overlap with both static (non-moving) and/or dynamic (moving) obstacles that the vehicle102may come across. The trajectory planning algorithm may simultaneously optimize a speed of the vehicle102and steering of the vehicle102to minimize control effort required to follow the person104within the surrounding environment of the vehicle102The vehicle control application106may thereby utilize outputs of the trajectory planning algorithm to provide nonlinear productive control of the vehicle102to follow the person104in different types of environments, including roadway environments, pathway environments, off-road environments, uneven ground environments, interior environments, and the like. For example, the vehicle control application106may utilize outputs of the trajectory planning algorithm to safely follow the person104on uneven grass, near obstacles, over ditches and curbs, on asphalt over train-tracks, and/or near buildings and automobiles.

With continued reference toFIG.1, in addition to the ECU110, the vehicle102may include a storage unit112, a communication unit114, a camera system116, and a laser projection system118. In an exemplary embodiment, the ECU110may execute one or more applications, operating systems, vehicle system and subsystem executable instructions, among others. In one or more embodiments, the ECU110may include a microprocessor, one or more application-specific integrated circuit(s) (ASIC), or other similar devices. The ECU110may also include an internal processing memory, an interface circuit, and bus lines for transferring data, sending commands, and communicating with the plurality of components of the vehicle102.

The ECU110may include a respective communication device (not shown) for sending data internally to components of the vehicle102and communicating with externally hosted computing systems (e.g., external to the vehicle102). Generally the ECU110may be operably connected to the storage unit112and may communicate with the storage unit112to execute one or more applications, operating systems, vehicle systems and subsystem user interfaces, and the like that are stored on the storage unit112. The storage unit112may be configured to store data associated with computer-implemented instructions associated with comprehensive trajectory planning for the vehicle102.

In one or more embodiments, the ECU110may be configured to operably control the plurality of components of the vehicle102. The ECU110may also provide one or more commands to one or more control units (not shown) of the vehicle102including, but not limited to, a motor/engine control unit, a braking control unit, a turning control unit, a transmission control unit, and the like to control the vehicle102to be autonomously operated. As discussed below, the ECU110may autonomously control the vehicle102based on one or more commands that are provided by the vehicle control application106upon the execution of the trajectory planning algorithm.

In one or more embodiments, the storage unit112may configured to store data, for example, one or more images, videos, one or more sets of image coordinates that may be provided by the camera system116and/or one or more sets of LiDAR coordinates associated with one or more persons (e.g., including the person104), static objects (e.g., including one or more static obstacles), and/or dynamic objects (e.g., including one or more dynamic obstacles) located within the surrounding environment of the vehicle102.

In an exemplary embodiment, the camera system116of the vehicle102may include one or more cameras that are positioned at one or more exterior portions of the vehicle102to capture the surrounding environment of the vehicle102(e.g., a vicinity of the vehicle102). The camera(s) of the camera system116may be positioned in a direction to capture the surrounding environment of the vehicle102that includes areas located around (front/sides/behind) the vehicle102. In one or more configurations, the one or more cameras of the camera system116may be disposed at external front, rear, and/or side portions of the including, but not limited to different portions of the bumpers, lighting units, body panels, and the like. The one or more cameras may be positioned on a respective planar sweep pedestal (not shown) that allows the one or more cameras to be oscillated to capture images of the surrounding environments of vehicle102.

In one embodiment, the camera system116may output image data that may be associated with untrimmed images/video of the surrounding environment of the vehicle102. In some embodiments, the vehicle control application106may be configured to execute image logic (e.g., pre-trained computer logic) to analyze the image data and determine vehicle based observations associated with the surrounding environment of the vehicle102. In some configurations, the vehicle control application106may be configured to analyze the image data using the image logic to classify and determine the position of one or more people, static objects, and/or dynamic objects that may be located within the surrounding environment of the vehicle102.

In an exemplary embodiment, the ECU110may also be operably connected to the laser projection system118of the vehicle102. The laser projection system118may include one or more respective LiDAR transceivers (not shown). The one or more respective LiDAR transceivers of the respective laser projection system118may be disposed at external front, rear, and/or side portions of bumpers, body panels, lighting units, and the like of the vehicle102.

The one or more respective LiDAR transceivers may include one or more planar sweep lasers that include may be configured to oscillate and emit one or more laser beams of ultraviolet, visible, or near infrared light toward the surrounding environment of the vehicle102. The laser projection system118may be configured to receive one or more reflected laser waves based on the one or more laser beams emitted by the LiDAR transceivers. For example, one or more reflected laser waves may be reflected off of the person104, one or more dynamic objects, one or more static objects, one or more boundaries (e.g., guardrails, walls) that may be located within the surrounding environment of the vehicle102.

In an exemplary embodiment, the laser projection system118may be configured to output LiDAR data that may be associated with the one or more reflected laser waves. In some embodiments, the vehicle control application106may receive the LiDAR data communicated by the laser projection system118and may execute LiDAR logic (e.g., pre-trained computer logic) to analyze the LiDAR data and determine LiDAR based observations associated with the surrounding environment of the vehicle102. In some configurations, the vehicle control application106may be configured to analyze the LiDAR data using the LiDAR logic to classify and determine the position of people, static objects, and/or dynamic objects that may be located within the surrounding environment of the vehicle102.

As discussed in more detail below, in one embodiment, the vehicle control application106may be configured to analyze the image data and/or the LiDAR data through execution of a perception algorithm. The perception algorithm may be configured to detect static and/or dynamic objects of interest such as the person104to be followed by the vehicle102and/or one or more obstacles that may be located within the surrounding environment of the vehicle102. The application106may be configured to input such detections and associated data to be utilized during execution of the trajectory planning algorithm.

With continued reference toFIG.1, in one embodiment, the communication unit114of the vehicle102may be operably controlled by the ECU110of the vehicle102. The communication unit114may be operably connected to one or more transceivers (not shown) of the vehicle102. The communication unit114may be configured to communicate through an internet cloud120through one or more wireless communication signals that may include, but may not be limited to Bluetooth® signals, Wi-Fi signals, ZigBee signals, Wi-Max signals, and the like.

In one embodiment, the communication unit114may be configured to connect to the internet cloud120to send and receive communication signals to and from the external server108. The external server108may host a neural network122that may be pre-trained with one or more datasets to detect the person104, additional persons (e.g., pedestrians), and/or obstacles that are located within the surrounding environment of the vehicle102. In one or more embodiments, the vehicle control application106may access the neural network122to process a programming model which enables computer/machine based/deep learning that may be centered on one or more forms of data that are inputted to the neural network122to provide inputs to execute the trajectory planning algorithm.

With continued reference to the external server108, the processor124may be operably connected to a memory126. The memory126may store one or more operating systems, applications, associated operating system data, application data, executable data, and the like. In one embodiment, the processor124of the external server108may additionally be configured to communicate with a communication unit128. The communication unit128may be configured to communicate through the internet cloud120through one or more wireless communication signals that may include, but may not be limited to Bluetooth® signals, Wi-Fi signals, ZigBee signals, Wi-Max signals, and the like.

In one embodiment, the communication unit128may be configured to connect to the internet cloud120to send and receive communication signals to and from the vehicle102. In particular, the external server108may receive image data and LiDAR data that may be communicated by the vehicle102based on the utilization of the camera system116and the laser projection system118. As discussed, such data may be inputted for perception to determine a goal of the vehicle102as the person104to be followed and one or more obstacles that may detected and inputted to the trajectory planner algorithm.

II. The Vehicle Control Application and Related Methods

The general functionality of the vehicle control application106will now be discussed in more detail with respect to exemplary methods that may be executed by the application106. In an exemplary embodiment, the vehicle control application106may be fully or partially executed by the ECU110of the vehicle102. Additionally or alternatively, the vehicle control application106may be fully or partially executed by the processor124of the external server108. The vehicle control application106may utilize the communication unit114of the vehicle102and the communication unit128of the external server108to communicate application related data between the vehicle102and the external server108.

FIG.2is a schematic view of the modules202-212of the vehicle control application106that may execute computer-implemented instructions for providing a comprehensive trajectory planner for a person-following vehicle according to an exemplary embodiment of the present disclosure. In an exemplary embodiment, the plurality of modules202-212may include a data reception module202, a perception module204, an encoder module206, a localization module208, a trajectory planning module210, and a vehicle control module212. However, it is appreciated that the vehicle control application106may include one or more additional modules and/or sub-modules that are included in addition to or in lieu of the modules202-212. The functionality of the plurality of modules202-212of the vehicle control application106will now be described in detail with reference to computer-executed methods that are executed by the vehicle control application106.

FIG.3is a process flow diagram of a method300for detecting persons, objects, and obstacles within the surrounding environment of the vehicle102based on image data and LiDAR data according to an exemplary embodiment of the present disclosure.FIG.3will be described with reference to the components ofFIG.1andFIG.2, through it is to be appreciated that the method300ofFIG.3may be used with additional and/or alternative system components.

The method300may begin at block302, wherein the method300may include receiving image data from the camera system116. In an exemplary embodiment, the data reception module202of the vehicle control application106may be configured to communicate with the camera system116to receive image data associated with one or more images of the surrounding environment of the vehicle102. As discussed above, the camera system116may output image data that may be associated with untrimmed images/video of the surrounding environment of the vehicle102.

The method300may proceed to block304, wherein the method300may include inputting the image data to the perception module204to detect the one or more pedestrians that are located within the surrounding environment of the vehicle102. In an exemplary embodiment, upon receiving the image data, the data reception module202may be configured to input the image data associated with the images of the surrounding environment of the vehicle102to the perception module204of the vehicle control application106. The perception module204may be configured to execute a perception algorithm that may be configured to analyze the image data input to determine persons and objects including the person104that is to be followed by the vehicle102as a goal.

FIG.4includes a schematic overview of an architecture of the sub-modules of the perception module204of the vehicle control application106according to an exemplary embodiment of the present disclosure. As shown inFIG.4, the perception module204may include sub-modules402,404,406that may be individually utilized to analyze the image data provided by the camera system116and the LiDAR data provided by the laser projection system118and may fuse such data to output fused data that includes goal data214associated with the person104to be followed and obstacle data216associated with one or more detected obstacles. In one embodiment, a pedestrian detection sub-module402may be configured to utilize the neural network122(e.g., configured as a convolutional neural network) to detect the person104to be followed by the vehicle102as a goal of the vehicle102.

In one configuration, the pedestrian detection sub-module402may utilize machine learning/deep learning capabilities of the neural network122to detect one or more pedestrians that may be in a field of view of one or more cameras of the camera system116as included within the image data. In one embodiment, the pedestrian detection sub-module402may analyze the image data as a region-proposal based object detector (e.g., which may have a similar structure to Faster-RCNN). As discussed above, the neural network122may be pre-trained with a propriety dataset. The pedestrian detection sub-module402may be configured to select an image frame (e.g., middle frame, last frame) from a plurality of image frames extracted from the image data. The plurality of image frames may be associated with images/video that are captured by one or more of the cameras for a predetermined period of time (e.g., three second clips).

Upon analysis of the image data through the neural network122based on the pre-trained dataset, the pedestrian detection sub-module402may be configured to output computed bounding boxes over the selected image frame from a plurality of image frames extracted from the image data. In particular, the bounding boxes may be computed to encapsulate pixels of the selected image frame that include one or more pedestrians that may be located within the surrounding environment of the vehicle102, as captured within the selected image frame. In addition to computing the bounding box locations of detected pedestrians, the pedestrian detection sub-module402may be configured to output a rough estimated distance between the vehicle102and each of the one or more detected pedestrians captured within the selected image frame.

In an exemplary embodiment, the pedestrian detection sub-module402may be configured to output pedestrian data that includes data pertaining to the one or more computed bounding box locations and the rough estimated distance between the vehicle102and each of the one or more detected pedestrians captured within the selected image frame. The pedestrian detection sub-module402may further analyze the one or more computed bounding box locations using image logic to determine the person104to be followed by the vehicle102as the goal of the vehicle102. In one embodiment, the pedestrian detection sub-module402may be configured to output goal data214associated with the location of the person104that is to be followed by the vehicle102.

Referring again toFIG.3, upon inputting the image data to the perception module204, the method300may proceed to block306, wherein the method300may include receiving LiDAR data from the laser projection system118. In an exemplary embodiment, the data reception module202of the vehicle control application106may be configured to communicate with the laser projection system118to receive LiDAR data that may be associated with the one or more reflected laser waves. In other words, the LiDAR data may include LiDAR based observations associated with the surrounding environment of the vehicle102.

The method300may proceed to block308, wherein the method300may include inputting the LiDAR data to the perception module204to detect one or more obstacles that are located within a predetermined distance of the vehicle102. In an exemplary embodiment, upon receiving the LiDAR data, the data reception module202may be configured to input the LiDAR data associated with the images of the surrounding environment of the vehicle102to the perception module204of the vehicle control application106. The perception module204may be configured to execute the perception algorithm that may be configured to analyze the LiDAR data to determine one or more obstacles that may be located within a predetermined distance of the vehicle102within the surrounding environment of the vehicle102.

Referring again toFIG.4, in one embodiment, upon receiving the LiDAR data, the LiDAR data may be analyzed by a dynamic occupancy sub-module404(DOM sub-module) of the perception module204. The DOM sub-module404may be configured to analyze the LiDAR data by filtering the LiDAR data to include one or more reflected laser waves that are reflected a maximum predetermined distance. For example, in one configuration, the DOM sub-module404may be configured to analyze the LiDAR data by filtering LiDAR data to include one or more reflected laser waves that are reflected a maximum distance of 25 m. The DOM sub-module404may thereby define obstacles based on the one or more reflected laser waves that are measured at a height over a predetermined height above the ground. For example, in one configuration, one or more obstacles may be defined as any object with a height over 0.7 m above the ground. In one embodiment, the output of the DOM sub-module404may include a list of convex hulls and each convex hull may describe an obstacle's spatial dimensions. In one embodiment, the DOM sub-module404may be configured to output obstacle data216associated with the location and spatial dimensions of one or more obstacles that are determined to be located within a predetermined distance of the vehicle102.

With continued reference toFIGS.3and4, the method300may proceed to block310, wherein the method300may include fusing pedestrian and obstacle detection results. In one configuration, the outputs of the pedestrian detection sub-module402based on the image based inputs and the DOM sub-module404based on the LiDAR based inputs may be inputted to a probabilistic sensor fusion sub-module406of the perception module204.FIG.5is a schematic overview of the probabilistic sensor fusion sub-module's architecture according to an exemplary embodiment of the present disclosure. In one embodiment, upon receiving the inputs pertaining to image based detections and the LiDAR based detections, the probabilistic sensor fusion sub-module406may be configured to fuse the pedestrian related data and obstacle related data into fused detections (detections)502.

In particular, upon receiving the inputs pertaining to pedestrian related data and obstacle related data, the probabilistic sensor fusion sub-module406may be configured to perform matching between incoming detections502and existing trackers504using a match sub-module506. The match sub-module506may be configured to execute a Hungarian algorithm, which is known in the art as a combinational optimization algorithm, to perform the matching between the incoming detections502and the existing trackers504.

In particular, in one configuration, the probabilistic sensor fusion sub-module406may define a cost function between the detections502and trackers504with respect to the LiDAR based detections as a Euclidean distance between detection and tracker centers as the cost. The probabilistic sensor fusion sub-module406may define a cost function between the detections502and trackers504with respect to the image based detections as a pixel distance between the projection of the tracker504onto an image plane and bounding box center as the cost. The matching may yield three types of outcomes. For a matched detection and tracker, the detection may be used to update the tracker. Unmatched trackers may be updated with negative (e.g., empty) detection. Additionally, the probabilistic sensor fusion sub-module406may allow unmatched detections to generate new trackers.

To fuse the two types of detections, the probabilistic sensor fusion sub-module406may model the existence probability Pexistof each tracked object. A probability update sub-module508of the probabilistic sensor fusion sub-module406may be configured to apply Bayes' Rule, to execute the equation of the Bayes' rule known in the art, to calculate an existence probability from an inverse sensor mode, P (existence I measurement). The probability update sub-module508may adopt a simple inverse sensor model by using certain false positive and false negative rates for the pedestrian data that includes data pertaining to the one or more computed bounding box locations and the rough estimated distance between the vehicle102and each of the one or more detected pedestrians captured within the selected image frame output by the pedestrian sub-module402. Additionally, the probability update sub-module508may adopt a simple inverse sensor model by using certain false positive and false negative rates with respect to the output of the DOM sub-module404that may include a list of convex hulls as each convex hull may describe an obstacle's spatial dimensions.

The Pexistmay be used to create new trackers and delete obsolete trackers. A tracker may be created whenever its Pexistexceeds a particular high threshold. This tracker is then deleted when its Pexistdrops below a particular low threshold. In one embodiment, a filter update sub-module510of the probabilistic sensor fusion sub-module406may be configured to estimate the position and velocity of every pedestrian (included within the selected image frame that is within the predetermined distance of the vehicle102) using a Kalman filter, known in the art, with a constant velocity model. The probabilistic sensor fusion sub-module406may thereby output fused data that includes goal data214associated with the person104to be followed and obstacle data216associated with one or more detected obstacles

FIG.6is a process flow diagram of a method600of executing the trajectory planning algorithm to output person following instructions to operably control the vehicle102to autonomously follow the person104according to an exemplary embodiment of the present disclosure.FIG.6will be described with reference to the components ofFIG.1andFIG.2through it is to be appreciated that the method600ofFIG.6may be used with additional and/or alternative system components. The method600may begin at block602, wherein the method600may include determining encoding data with respect to the vehicle state. In one embodiment, the encoder module206of the vehicle control application106may be configured to determine encoder data based on vehicle dynamic data that may be communicated by the ECU110of the vehicle102. The vehicle dynamic data may be determined from vehicle dynamic sensors (not shown) of the vehicle102based on the communication such data to the ECU110. The vehicle dynamic data may include, but may not be limited to, steering data and speed data associated with one or more steering angles and speed of the vehicle102. Accordingly, the encoder module206may determine speed encoders and steering encoders.

The method600may proceed to block604, wherein the method600may include determining a vehicle state218based on execution of a localization algorithm. In an exemplary embodiment, upon determining the speed encoders and the steering encoders, the encoder module206may be configured to communicate respective data to the localization module208of the vehicle control application106to output a vehicle state218of the vehicle102. The localization module208may execute localization algorithms, known in the art, which may extract and use data from the speed encoders and the steering encoders to estimate the vehicle state218of the vehicle102using an odometry-based estimate.

The method600may proceed to block606, wherein the method600may include executing the trajectory planning algorithm to output data associated with person following instructions. In an exemplary embodiment, the perception module204may input the fused data that includes the goal data214associated with the detection of the person104to be followed by the vehicle102and obstacle data216associated with the detection of one or more obstacles located within a predetermined distance of the vehicle102to the trajectory planning module210of the vehicle control application106. In an exemplary embodiment, the trajectory planning module210may be configured to execute the trajectory planning algorithm to overcome an optimization problem (e.g., optimal control problem) that may be formulated to incorporate the planner specifications of person following behavior, static and moving obstacle avoidance, suitability for front-wheel steering, optimization of speed and steering, and the utilization of minimum control effort. The optimization problem may be formulated as:

In an exemplary embodiment, the cost functional of the equation +ws0ξ0s+wsfξfsis given as:

In one embodiment, by adding a minimum final time term, the trajectory planning algorithm may calculate more aggressive trajectories, which may enable the vehicle102to move towards the goal using a shortest possible path. In an exemplary embodiment, the trajectory planning module210may model the vehicle102using a nonlinear kinematic ground vehicle model to thereby model dynamic constriaints of the vehicle102. The dynamic constrains of the above stated equation

d⁢ξd⁢t⁢(t)-f⁡(ξ⁡(t),ζ⁡(t),t)=0
may be defined using a kinematic vehicle as:

x˙(t)=ux(t)⁢cos⁡(ψ⁡(t)+tan⁡(la⁢tan⁡(δf(t))la+lb)-1)y.(t)=ux(t)⁢sin⁡(ψ⁡(t)+tan⁡(lα⁢tan⁡(δf(t))la+lb)-1)ψ.(t)=ux(t)⁢sin⁡(tan⁡(la⁢tan⁡(δf(t))la+lb)-1)lbu.x(t)=ax(t)
Where ψ(t) is the yaw angle, lamay equal n wheel base distance meters (e.g., 0.6 m) and lbmay equal n wheel base distance meters (e.g., 0.6 m) as wheel base distances of the vehicle102.

In one embodiment, the trajectory planning module210may define path constraints to avoid overlap between the path of the vehicle102and static obstacles and/or dynamic obstacles located within the surrounding environment of the vehicle102. Accordingly, the trajectory planning module210may execute time-varying hard constraints on the vehicle's trajectory to ensure that the vehicle's planed trajectory does not intersect with one or more obstacles' (that are located within the surrounding environment) predicted trajectories. The path constraints of the above mentioned equation: C(ξ(t), ζ(t), t)≤0 are as follows:

s⁢m⁡(t)=0.4⁢5+0.7-0.45tf⁢t
describes the time-varying safety margin, xoobs[] and yoobs[] describe the positon of the center of the ith obstacle at time t, aobsand bobsare arrays of semi-major and semi-minor obstacles' axis, and Q is the number of obstacles.

Referring again to the method600ofFIG.6, upon executing the trajectory planning algorithm, the method600may proceed to block608, wherein the method600may include operably controlling the vehicle102to autonomously follow the person104. Based on the execution of the trajectory planning algorithm, the trajectory planning module210may output steering angle δf(t) and steering rate sr(t), the velocity ux(t) (longitudinal speed) to the vehicle control module212of the vehicle control application106. In some configurations, the trajectory planning module210may also output x(t) and y(t) as the vehicle's position coordinates, xgand ygas the coordinates of the person104as the goal of the vehicle102, ψgas the desired final heading angle, and ax(t) as the longitudinal acceleration. Additionally, based on the execution of the localization algorithm (at block604), the localization module208may output the vehicle state218ε0of the vehicle102to the vehicle control module212.

In an exemplary embodiment, the vehicle control module212may evaluate numerous data points communicated to the vehicle control module212by the trajectory planning module210and the localization module208. The vehicle control module212may be configured to communicate with the ECU110of the vehicle102to operably control the motor/engine control unit, the braking control unit, the turning control unit, the transmission control unit, and the like to control the vehicle102to be autonomously operated to follow the person104while ensuring static and moving obstacle avoidance, suitability for front-wheel steering, optimization of the speed and steering of the vehicle102, while using minimum control effort in navigating the vehicle102.

FIG.7is a process flow diagram of a method700method for providing a comprehensive trajectory planner for a person-following vehicle according to an exemplary embodiment of the present disclosure.FIG.7will be described with reference to the components ofFIG.1andFIG.2, through it is to be appreciated that the method700ofFIG.7may be used with additional and/or alternative system components. The method700may begin at block702, wherein the method700may include receiving image data and LiDAR data associated with a surrounding environment of a vehicle102.

The method700may proceed to block704, wherein the method700may include analyzing the image data and detecting the person to be followed that is within an image. The method700may proceed to block706, wherein the method700may include analyzing LiDAR data and detecting an obstacle that is located within a predetermined distance from the vehicle102. The method700may proceed to block708, wherein the method700may include executing a trajectory planning algorithm based on fused data associated with the detected person and the detected obstacle. In one embodiment, the trajectory planning algorithm utilizes nonlinear model predictive control to enable the vehicle102to follow the person within the surrounding environment of the vehicle102.

It should be apparent from the foregoing description that various exemplary embodiments of the disclosure may be implemented in hardware. Furthermore, various exemplary embodiments may be implemented as instructions stored on a non-transitory machine-readable storage medium, such as a volatile or non-volatile memory, which may be read and executed by at least one processor to perform the operations described in detail herein. A machine-readable storage medium may include any mechanism for storing information in a form readable by a machine, such as a personal or laptop computer, a server, or other computing device. Thus, a non-transitory machine-readable storage medium excludes transitory signals but may include both volatile and non-volatile memories, including but not limited to read-only memory (ROM), random-access memory (RAM), magnetic disk storage media, optical storage media, flash-memory devices, and similar storage media.