# US 11,029,697 B2 · Systems and methods for vehicular navigation > Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt - Number: US 11,029,697 B2 - Document ID: US11029697B2 - Status: Granted - Kind: B2 - Application number: 16/387,676 - Filed: 2019-04-18 - Published: 2020-07-09 - Granted: 2021-06-08 - Earliest claimed: 2019-01-07 - Assignees: Toyota Research Institute, Inc. - Applicants: Toyota Research Institute, Inc. - Inventors: Alexander G. Cunningham; Robert A. E. Zidek; Noah J. Epstein - Web page: https://patentgrep.com/patent/US11029697B2 ## CPC - G01C 21/3407: {specially adapted for specific applications} - G05D 1/0238: {using obstacle or wall sensors (G05D1/0246 and G05D1/0289 take precedence; lidar systems designed for anti-collision purposes G01S17/93)} - B60W 50/0097: {Predicting future conditions} - B60W 50/0205: {Diagnosing or detecting failures; Failure detection models} - B60W 50/029: Adapting to failures or work around with other constraints, e.g. circumvention by avoiding use of failed parts - B60W 50/14: Means for informing the driver, warning the driver or prompting a driver intervention - G01C 21/3602: {Input other than that of destination using image analysis, e.g. detection of road signs, lanes, buildings, real preceding vehicles using a camera} - G05D 1/0055: {with safety arrangements} - G05D 1/0088: {characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours (using knowledge based models G06N5/00)} - G05D 1/0212: {with means for defining a desired trajectory (involving a plurality of land vehicles G05D1/0287)} - G06V 20/58: Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads - G06V 20/588: {Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road} - G07C 5/0808: {Diagnosing performance data (testing of vehicles G01M17/00; testing of electrical installation on vehicles G01R31/005)} - B60W 2050/0083: {Setting, resetting, calibration} - B60W 2050/021: {Means for detecting failure or malfunction} - B60W 2050/0215: {Sensor drifts or sensor failures} - B60W 2050/146: {Display means} - G05D 1/0231: {using optical position detecting means (position-fixing by using electromagnetic waves other than radio waves, e.g. optical position detecting means G01S5/16)} - G05D 1/0255: {using acoustic signals, e.g. ultra-sonic singals (sonar systems designed for anti-collision purposes G01S15/93)} - G05D 1/0257: {using a radar (radar systems designed for anti-collision purposes between land vehicles or between land vehicle and fixed obstacles G01S13/931)} - G01C: MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY (measuring liquid level G01F; radio navigation, determining distance or velocity by use of propagation effects, e.g. Doppler effects, propagation time, of radio waves, analogous arrangements using other waves G01S) - G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES - B60W: CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT - G06V: IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING - G07C: TIME OR ATTENDANCE REGISTERS; REGISTERING OR INDICATING THE WORKING OF MACHINES; GENERATING RANDOM NUMBERS; VOTING OR LOTTERY APPARATUS; ARRANGEMENTS, SYSTEMS OR APPARATUS FOR CHECKING NOT PROVIDED FOR ELSEWHERE ## Abstract Systems and methods described herein relate to vehicular navigation. One embodiment generates a polyline reference path for a vehicle; stores a representation of the polyline in a data structure; detects a plurality of obstacles ahead of the vehicle; identifies one or more obstacle gates among the plurality of obstacles using path coordinates relative to the reference path, each obstacle gate including at least one cluster of obstacles; identifies one or more gaps within each of the one or more obstacle gates; determines an obstacle-avoidance path for the vehicle that passes through a particular one of the one or more gaps in each of the one or more obstacle gates; and controls one or more aspects of operation of the vehicle based, at least in part, on the obstacle-avoidance path. ## Claims ### Claim 1 A system for vehicular navigation, the system comprising: one or more sensors to produce sensor data; one or more processors; and a memory communicably coupled to the one or more processors and storing: an obstacle-avoidance module including instructions that when executed by the one or more processors cause the one or more processors to: generate a reference path for a vehicle for at least a portion of a route, wherein the reference path is a polyline including a plurality of nodes connected by line segments, the plurality of nodes including an origin node; store a representation of the polyline in a data structure that includes a plurality of cells, the plurality of cells corresponding, respectively, to the plurality of nodes, each cell in the plurality of cells having a corresponding index and storing contents including a signed arclength to the origin node; detect, based on the sensor data, a plurality of obstacles ahead of the vehicle along the route; identify one or more obstacle gates among the plurality of obstacles using path coordinates relative to the reference path, each obstacle gate including at least one cluster of obstacles, the path coordinates of a given obstacle in the plurality of obstacles including an index corresponding to a particular cell in the plurality of cells, the signed arclength to the origin node stored in the particular cell, and a lateral offset from the reference path; identify one or more gaps within each of the one or more obstacle gates; and determine an obstacle-avoidance path for the vehicle that passes through a particular one of the one or more gaps in each of the one or more obstacle gates; and a control module including instructions that when executed by the one or more processors cause the one or more processors to control one or more aspects of operation of the vehicle based, at least in part, on the obstacle-avoidance path. ### Claim 2 (depends on 1) The system of claim 1, wherein the reference path lies within a single lane of a roadway. ### Claim 3 (depends on 1) The system of claim 1, wherein the obstacle-avoidance module includes further instructions to: expand a particular obstacle gate among the one or more obstacle gates to span a lane adjacent to a lane in which the vehicle is currently traveling; identify a gap within the expanded particular obstacle gate; and determine an obstacle-avoidance path that passes through the gap within the expanded particular obstacle gate. ### Claim 4 (depends on 1) The system of claim 1, wherein the data structure is an array and the contents of each cell in the plurality of cells include a distance to a next node in the polyline. ### Claim 5 (depends on 1) The system of claim 1, wherein the instructions included in the obstacle-avoidance module to identify one or more gaps among the one or more obstacle gates include instructions to account for a width of the vehicle and for padding within at least one of a right lane boundary and a left lane boundary of a lane of a roadway. ### Claim 6 (depends on 1) The system of claim 1, wherein the instructions included in the obstacle-avoidance module to determine the obstacle-avoidance path for the vehicle include instructions to: represent the obstacle-avoidance path as an obstacle-avoidance polyline in terms of path coordinates; and apply a smoothing algorithm to the obstacle-avoidance polyline to generate a smoothed obstacle-avoidance path. ### Claim 7 (depends on 1) The system of claim 1, wherein the plurality of obstacles include at least one of other vehicles, construction cones, construction equipment, barricades, and debris. ### Claim 8 (depends on 1) The system of claim 1, wherein the instructions included in the control module to control one or more aspects of operation of the vehicle based, at least in part, on the obstacle-avoidance path include instructions to display the one or more obstacle gates and the obstacle-avoidance path on a display device of the vehicle. ### Claim 9 (depends on 8) The system of claim 8, wherein the instructions to display the one or more obstacle gates and the obstacle-avoidance path on the display device of the vehicle cause the one or more obstacle gates and the obstacle-avoidance path to be displayed in a manner that assists a human driver in steering the vehicle along the obstacle-avoidance path while the vehicle is operating in a manual driving mode. ### Claim 10 (depends on 1) The system of claim 1, wherein the instructions included in the control module to control one or more aspects of operation of the vehicle based, at least in part, on the obstacle-avoidance path include instructions to steer the vehicle automatically in accordance with the obstacle-avoidance path while the vehicle is operating in an autonomous driving mode. ### Claim 11 (depends on 1) The system of claim 1, wherein the one or more sensors include at least one of a camera, a Light Detection and Ranging (LIDAR) sensor, a radar sensor, and a sonar sensor. ### Claim 12 A non-transitory computer-readable medium for vehicular navigation and storing instructions that when executed by one or more processors cause the one or more processors to: generate a reference path for a vehicle for at least a portion of a route, wherein the reference path is a polyline including a plurality of nodes connected by line segments, the plurality of nodes including an origin node; store a representation of the polyline in a data structure that includes a plurality of cells, the plurality of cells corresponding, respectively, to the plurality of nodes, each cell in the plurality of cells having a corresponding index and storing contents including a signed arclength to the origin node; detect, based on sensor data, a plurality of obstacles ahead of the vehicle along the route; identify one or more obstacle gates among the plurality of obstacles using path coordinates relative to the reference path, each obstacle gate including at least one cluster of obstacles, the path coordinates of a given obstacle in the plurality of obstacles including an index corresponding to a particular cell in the plurality of cells, the signed arclength to the origin node stored in the particular cell, and a lateral offset from the reference path; identify one or more gaps within each of the one or more obstacle gates; determine an obstacle-avoidance path for the vehicle that passes through a particular one of the one or more gaps in each of the one or more obstacle gates; and control one or more aspects of operation of the vehicle based, at least in part, on the obstacle-avoidance path. ### Claim 13 A method of vehicular navigation, the method comprising: generating a reference path for a vehicle for at least a portion of a route, wherein the reference path is a polyline including a plurality of nodes connected by line segments, the plurality of nodes including an origin node; storing a representation of the polyline in a data structure that includes a plurality of cells, the plurality of cells corresponding, respectively, to the plurality of nodes, each cell in the plurality of cells having a corresponding index and storing contents including a signed arclength to the origin node; detecting, based on sensor data, a plurality of obstacles ahead of the vehicle along the route; identifying one or more obstacle gates among the plurality of obstacles using path coordinates relative to the reference path, each obstacle gate including at least one cluster of obstacles, the path coordinates of a given obstacle in the plurality of obstacles including an index corresponding to a particular cell in the plurality of cells, the signed arclength to the origin node stored in the particular cell, and a lateral offset from the reference path; identifying one or more gaps within each of the one or more obstacle gates; determining an obstacle-avoidance path for the vehicle that passes through a particular one of the one or more gaps in each of the one or more obstacle gates; and controlling one or more aspects of operation of the vehicle based, at least in part, on the obstacle-avoidance path. ### Claim 14 (depends on 13) The method of claim 13, wherein the reference path lies within a single lane of a roadway. ### Claim 15 (depends on 13) The method of claim 13, further comprising: expanding a particular obstacle gate among the one or more obstacle gates to span a lane adjacent to a lane in which the vehicle is currently traveling; identifying a gap within the expanded particular obstacle gate; and determining an obstacle-avoidance path that passes through the gap within the expanded particular obstacle gate. ### Claim 16 (depends on 13) The method of claim 13, wherein identifying one or more gaps among the one or more obstacle gates includes accounting for a width of the vehicle and for padding within at least one of a right lane boundary and a left lane boundary of a lane of a roadway. ### Claim 17 (depends on 13) The method of claim 13, wherein the obstacle-avoidance path is represented as an obstacle-avoidance polyline in terms of path coordinates and the method further comprises: applying a smoothing algorithm to the obstacle-avoidance polyline to generate a smoothed obstacle-avoidance path. ### Claim 18 (depends on 13) The method of claim 13, wherein controlling one or more aspects of operation of the vehicle based, at least in part, on the obstacle-avoidance path includes displaying the one or more obstacle gates and the obstacle-avoidance path on a display device of the vehicle. ### Claim 19 (depends on 18) The method of claim 18, wherein the one or more obstacle gates and the obstacle-avoidance path are displayed in a manner that assists a human driver in steering the vehicle along the obstacle-avoidance path while the vehicle is operating in a manual driving mode. ### Claim 20 (depends on 13) The method of claim 13, wherein controlling one or more aspects of operation of the vehicle based, at least in part, on the obstacle-avoidance path includes steering the vehicle automatically in accordance with the obstacle-avoidance path while the vehicle is operating in an autonomous driving mode.