WO2023119290A1

WO2023119290A1 - Automatic speed control in a vehicle

Info

Publication number: WO2023119290A1
Application number: PCT/IL2022/051372
Authority: WO
Inventors: Ofir COHEN; Dina Appelman
Original assignee: Elta Systems Ltd.
Priority date: 2021-12-23
Filing date: 2022-12-22
Publication date: 2023-06-29

Abstract

According to examples of the presently disclosed subject matter, while a vehicle is advancing in a certain area, autonomous detection of vehicle instability is implemented. If vehicle instability is detected, it is determined whether the current vehicle's speed deviates from an acceptable vehicle speed or speed range, and if it does the vehicle speed is adapted to be within the acceptable values.

Description

AUTOMATIC SPEED CONTROL IN A VEHICLE

TECHNICAL FIELD

The presently disclosed subject matter relates to autonomous control of vehicles.

BACKGROUND

An autonomous vehicle (AV), otherwise referred to as unmanned vehicle (UV), or an uncrewed vehicle, is a motorized machine that travels by integrating sensory data with computer-based decision-making, for the purpose of autonomously driving the vehicle. An autonomous vehicle can therefore travel without, or with limited, human assistance. One example of an autonomous vehicle is an unmanned ground vehicle (UGV) configured for autonomous travel on the ground.

During autonomous operation of the autonomous vehicle, various systems and devices onboard the vehicle operate for controlling the vehicle and leading it to a desired destination within a traversed area. This includes, for example, sensing devices operatively connected to a navigation computer, configured to determine localization data indicative of the position of the vehicle relative to the environment (in a local and/or global frame of reference), and generate, based on the localization data, instructions indicating how the vehicle should advance. The instructions are implemented by the vehicle control sub-systems that control the vehicle control devices and drive the vehicle.

GENERAL DESCRIPTION

Autonomous operation of a vehicle (e.g., an unmanned ground vehicle; UGV) includes in some examples, terrain mapping, path planning and vehicle navigation and control. Terrain mapping includes generating of a digital representation of the area in which it travels, such as a map, the representation comprising data indicating the presence of obstacles, where an obstacle is any area or object which should be avoided. During path planning, the map is used for planning a path which leads to a target destination while avoiding obstacles. During vehicle navigation, steering commands are generated for leading the vehicle according to the map.

Obstacles include any object or area which either blocks or endangers the vehicle. For example, obstacles may include positive obstacles, which are above ground level (e.g., walls, rocks, boulders, structures, etc.), negative obstacles which are below ground level (e.g., holes in the ground, water reservoirs, crevices, etc.), overhead obstacles (e.g., overhead bridges, trees, etc.), and inclined terrain (e.g., steep slopes, ramps, etc.). Obstacles can also include any area which is desired to be avoided for any reason, for example, noxious areas, habitat areas which should be avoided for ecological or animal conservation reasons, populated areas which should be avoided for safety reasons, etc. All such obstacles may be non-traversable for a vehicle and in such cases should be avoided. Notably, an object, landcover, or topography, may be considered an obstacle when approached from one direction, but not when approached from another direction. In addition to obstacle avoidance, path planning also strives to comply with other requirements, for example efficiency, e.g., avoiding paths which are unnecessarily long or winding. Once a path is planned or updated, it is followed by generating and executing vehicle control commands (e.g., steering commands) directed for controlling the vehicle along the path.

The presently disclosed subject matter includes a system and method of autonomous real-time control over a vehicle (for example an unmanned vehicle such as an unmanned ground vehicle). During operation of the autonomous vehicle, due to various environmental conditions which exist in the area that is being traversed, the vehicle may exhibit deficient performance. For example, a vehicle advancing in unstable conditions may lose traction and skid over the surface. Unstable conditions may occur, for example, when the vehicle is traveling on a wet, icy, snowy, or oil- stained road, or when the vehicle is traveling in wet, muddy, snowy, or sandy terrain, or over gravel. Vehicle instability may result in inaccurate determination of the vehicle's position and/or its location relative to the environment, and may, in some cases, lead to a catastrophic event such as an accident and/or vehicle malfunction, and therefore should be avoided.

According to examples of the presently disclosed subject matter, while the vehicle is advancing in a certain area, autonomous detection of vehicle instability is implemented. If vehicle instability is detected, the speed of the autonomous vehicle is compared to a respective threshold value representing an acceptable vehicle speed while the autonomous vehicle is experiencing the detected vehicle instability. If it is determined that the current vehicle's speed deviates from the acceptable vehicle speed or speed range, the vehicle speed is adapted to be within the acceptable values.

According to one aspect of the presently disclosed subject matter there is provided a method of speed-control over a vehicle comprising a self-contained navigation system (SCNS, e.g., an INS), the method comprising: while the vehicle is advancing through an area: i. determining a first current vehicle state based on SCNS data obtained from the SCNS, the first current vehicle state including at least a first current vehicle position (e.g., in a relative reference frame); ii. predicting, based on the first current vehicle state, a first future vehicle state following a first time-window; wherein the first future vehicle state comprises at least a first future vehicle position relative to the first current vehicle position; following the first time-window: iii. determining a second current vehicle state based on SCNS data obtained from the SCNS, the second current vehicle state including at least a second current vehicle position relative to the first current vehicle position; and iv. if a difference between the first future vehicle position and the second current vehicle position is greater than an instability threshold value, detecting vehicle instability; and v. generating a command for applying speed limitation that limits the vehicle's speed to a maximal speed threshold allowed under instability conditions.

In addition to the above features, the method according to this aspect of the presently disclosed subject matter can optionally comprise one or more of features (a) to r) listed below, in any technically possible combination or permutation: a) The method further comprising: if the speed of the vehicle is greater than the maximal speed threshold, generating a speed control command to a speed control subsystem for reducing the speed of the vehicle. b) The method further comprising: if a difference between the first future vehicle position and the second current vehicle position is lower than the instability threshold value, detecting vehicle stability; and if speed limitation is currently applied, generating a command for removing speed limitation and allowing to increase the vehicle's speed. c) wherein, if the speed of the vehicle is lower than the maximal threshold, generating a speed control command to a speed control subsystem for increasing the speed of the vehicle. d) Wherein generating a speed control command comprises: generating incremental commands, each incremental command dedicated for increasing the speed of the vehicle by an incremental value. e) The method further comprising: following execution of each incremental command, repeating operations i-iii; if a difference between the first future vehicle position and the second current vehicle position is lower than the instability threshold value, generating a further incremental command dedicated for further increasing the speed of the vehicle by an incremental value; otherwise, applying speed limitation that limits the vehicle's speed to a maximal speed threshold allowed under instability conditions. f) wherein the instability threshold is determined according to an estimated risk to the vehicle (e.g., risk form the environment in which it travels). g) The method further comprising: processing imaging data of an area surrounding the vehicle and determining a risk score indicating a level risk to the vehicle while traversing the area and adapting the instability threshold value according to the risk score. h) wherein the processing comprises: as the vehicle is advancing through the area, operating a scanning device (e.g., camera or laser scanner) located onboard the vehicle for capturing the imaging data. i) The method comprising processing the imaging data and determining obstacle density and dynamically adapting in real-time the instability threshold value according to the obstacle density, wherein detection of a greater obstacle density is followed by decreasing the instability threshold value, and vice versa. j) The method further comprising: repeating operations i-iv for a plurality of consecutive time-windows; and performing operation v if the portion of time-windows, out of the plurality of time-windows in which vehicle instability is detected, is greater than a certain value. k) wherein the vehicle exhibits a delay in execution of navigation commands generated for navigating the vehicle, wherein the first future vehicle state is predicted based on accumulated effects of execution of one or more navigation commands pending execution during the first time window. l) wherein the first current vehicle position, the first future vehicle position and the second current vehicle position are determined in a relative (local) coordinate system. m) wherein the vehicle is an autonomous vehicle that travels autonomously in the area. n) wherein the SCNS is an inertial navigation system. o) The method further comprising executing the speed control command and reducing the speed of the vehicle. p) Wherein the command for applying speed limitation is a warning advising an operator to reduce the speed of the vehicle. q) The method further comprising reducing the speed of the vehicle in response to a command received by an operator following the warning. r) The method further comprising operating a scanning device onboard the vehicle for repeatedly executing a scanning operation, where each scanning operation includes scanning an area surrounding the vehicle, to thereby generate respective scanning output data; operating the at least one processor for generating, based on the scanning output data from a plurality of scanning operations, a map representing at least a part of the area, the map being relative to a location of the vehicle and is characterized by a certain size, wherein the map comprises cells, each cell being classified to a class selected from at least two classes, comprising traversable and non-traversable, and characterized by dimensions equal to or larger than an accumulated drift value of the INS over a predefined distance (the distance is derived, for example, from one or more dimensions of the map); wherein non-traversable cells correspond to obstacles; receiving SCNS (e.g., INS) data indicative of a current location of the vehicle relative to a previous location; and updating a location of the vehicle relative to cells in the map based on the SCNS data; and controlling movement of the vehicle within the area based on the map and the location of the vehicle relative to the cells determined based on the received INS data, in order to avoid non-traversable cells.

According to another aspect of the presently disclosed subject matter there is provided a system mountable on a vehicle (e.g., an autonomous vehicle) for providing automatic speed control over the vehicle, the system comprising a self-contained navigation system (SCNS) and at least one processing circuitry configured to: determine a first current vehicle state based on SCNS data obtained from the SCNS, the first current vehicle state including at least a first current vehicle position (relative to previous position); predict, based on the first current vehicle state, a first future vehicle state following a first time window; wherein the first future vehicle state comprises at least a first future vehicle position relative to the first current vehicle position; following the first time-window: determine a second current vehicle state based on SCNS data obtained from the SCNS, the second current vehicle state including at least a second current vehicle position relative to the first current vehicle position; and detect vehicle instability if a difference between the first future vehicle position and the second current vehicle position isgreaterthan an instability threshold value; and generate a command for applying speed limitation that limits the vehicle's speed to a maximal speed threshold allowed under instability conditions.

According to another aspect of the presently disclosed subject matter there is provided a vehicle comprising the system according to the previous aspect, where in some examples the vehicle is an autonomous vehicle.

According to another aspect of the presently disclosed subject matter there is provided a computer program product comprising a computer readable storage medium retaining program instructions, which programs instructions when read by a processor and causes the processor to perform a method of speed-control over a vehicle comprising a self-contained navigation system (SCNS, e.g., an INS), the method comprising: while the vehicle is advancing through an area: i. determining a first current vehicle state based on SCNS data obtained from the SCNS, the first current vehicle state including at least a first current vehicle position; ii. predicting, based on the first current vehicle state, a first future vehicle state following a first time-window; wherein the first future vehicle state comprises at least a first future vehicle position relative to the first current vehicle position; following the first time-window: iii. determining a second current vehicle state based on SCNS data obtained from the SCNS, the second current vehicle state including at least a second current vehicle position relative to the first current vehicle position; and iv. if a difference between the first future vehicle position and the second current vehicle position is greater than an instability threshold value, detecting vehicle instability; and v. generating a command for applying speed limitation that limits the vehicle's speed to a maximal speed threshold allowed under instability conditions.

The computer program product, the system and vehicle disclosed in accordance with the aspects of the presently disclosed subject matter detailed above can optionally comprise one or more of features (a) to (r) listed above, mutatis mutandis, in any technically possible combination or permutation. BRIEF DESCRIPTION OF THE DRAWINGS

In order to understand the invention and to see how it can be carried out in practice, embodiments will be described, by way of non-limiting examples, with reference to the accompanying drawings, in which:

Fig. 1 illustrates a schematic block diagram of a system mountable on a vehicle, in accordance with certain examples of the presently disclosed subject matter;

Fig. 2 illustrates graphically some principles related to instability detection, in accordance with certain examples of the presently disclosed subject matter;

Fig. 3A illustrates a first flow-chart of operations carried out, in accordance with certain examples of the presently disclosed subject matter; and

Fig. 3B illustrates a second flow-chart of operations carried out, in accordance with certain examples of the presently disclosed subject matter.

DETAILED DESCRIPTION

In the following detailed description, numerous specific details are set forth in order to provide a thorough understanding of the invention. However, it will be understood by those skilled in the art that the presently disclosed subject matter may be practiced without these specific details. In other instances, well-known methods, procedures, components and circuits have not been described in detail so as not to obscure the presently disclosed subject matter.

Unless specifically stated otherwise, as apparent from the following discussions, it is appreciated that throughout the specification discussions utilizing terms such as "predicting", "estimating", "generating", "determining", "processing", "aligning", or the like, refer to the action(s) and/or process(es) of a computer that manipulate and/or transform data into other data, said data represented as physical, such as electronic, quantities and/or said data representing the physical objects.

The terms "computer", "processing unit" or the like should be expansively construed to include any kind of electronic device with one or more data processing circuitry, which includes a computer processor as disclosed herein below (e.g., a Central Processing Unit (CPU), a microprocessor, an electronic circuit, an Integrated Circuit (IC), firmware written for or ported to a specific processor such as a digital signal processor (DSP), a microcontroller, a field programmable gate array (FPGA), an application specific integrated circuit (ASIC), etc.) and possibly a computer memory, and is capable of executing various data processing operations.

Some of the operations in accordance with the teachings herein may be performed by a computer device specially constructed for the desired purposes, or by a general-purpose computer specially configured for the desired purpose by a computer program stored in a computer readable storage medium.

Fig. 1 illustrates a general schematic block diagram of a system mountable on a vehicle in accordance with some examples of the presently disclosed subject matter. Different components in Fig. 1 can be made up of any combination of software, hardware and/or firmware that performs the functions as defined and explained herein. The components in Fig. 1 may be centralized in one location or on one device, or dispersed over more than one location or device. In different examples of the presently disclosed subject matter, the system may comprise fewer, more, and/or different modules than those shown in Fig. 1. For example, while Fig. 1 illustrates navigation computer 120 comprising a single processing circuitry, this is done by way of example and multiple inter-connected processing circuitries can be used instead.

Processing circuitry illustrated in Fig. 1 as part of navigation computer 120 can be configured, for example, to execute several functional modules in accordance with computer-readable instructions implemented on a non-transitory computer-readable storage medium. Such functional modules are referred to hereinafter as comprised in the processing circuitry. Figs. 3A and 3B are flowcharts illustrating operations carried out in accordance with examples of the presently disclosed subject matter. According to different examples of the presently disclosed subject matter, fewer, more and/or different stages than those shown in Figs. 3A and 3B may be executed. According to different examples of the presently disclosed subject matter one or more stages illustrated in Figs. 3A and 3B may be executed in a different order and/or one or more groups of stages may be executed simultaneously for multiple commands.

As described below a vehicle (e.g., an autonomous vehicle) in accordance with the description may be equipped with a scanning device configured to scan the area surrounding the UGV and one or more self-contained navigation systems (SCNSs) configured to provide navigation parameters of the UGV, such as position, attitude and altitude.

The term "scanning device" as used herein should be expansively construed to include any kind of device configured to scan an area surrounding the UGV, and identify objects in the vicinity of the device, their distance and direction relative to the device. Examples of scanning devices include, but are not limited to, laser scanners (including LIDAR), RADAR, image sensor (camera), sonar, etc. A scanning device can scan for example, 360° on a plane surrounding the device, or in some other smaller scanning angle (e.g. 180°). Alternatively, the scanning device can scan a volume such as a sphere, a cylinder, or another three-dimensional volume or part thereof in the vicinity of the AV. A scanning device can provide information for generating a 3- dimensional map of the scanned area. In some examples, to save resources, a 2.5- dimensional map can be generated, where the map is divided into cells, and each cell is assigned with a single respective altitude value calculated based on multiple altitude measurements made within the area of the cell (e.g., average value of the altitude measurements).

While the following description predominantly refers to an Inertial Navigation System (INS) this is done by way of example only and should not be construed as limiting. The presently disclosed subject matter contemplates a self-contained navigation system (SCNS; otherwise known as "dead-reckoning device") other than INS, that applies dead reckoning, independent of an external reference. This is different, for example, from GPS navigation that is dependent on information obtained from satellites. Other types of self-contained navigation systems include for example, Simultaneous Localization and Mapping (SLAM) and odometer navigation systems. Like INS, other self-contained navigation systems also suffer from drift that accumulates as the vehicle advances.

Reference is now made to Fig. 1, showing a schematic block diagram of a system mountable on a vehicle (e.g., an autonomous vehicle such as a UGV), in accordance with some examples of the disclosure. System 100 can comprise various sensing subsystems and/or devices 110 including, for example, scanning device 101, self-contained navigation systems such as INS 103 and odometer 105. Scanning device 101 is configured to scan an area surrounding the vehicle and provide scanning output data used for generating maps and/or obstacle detection, as further described below.

INS 103 calculates the kinematic parameters of the vehicle based upon readings received from an Inertial Measurement Unit (IMU). A common IMU configuration includes one accelerometer and one gyro per each of the three platform axes: pitch, roll and yaw. The IMU detects linear acceleration in each axis using the accelerometers and rotation rate in each axis using the gyroscopes. Some IMUs also include magnetometers (likewise, one per each axis) which are used for determining a magnetic field in each axis. The INS includes a processing circuitry configured to utilize the raw IMU measurements for calculating navigation data including for example, position, velocity, and attitude without the need for external references. An INS calculates navigation data by dead reckoning, where current position of the vehicle is calculated based on a previously determined position, or fix, and then incorporating estimates of speed, heading direction, and course, over elapsed time.

In some examples sensing subsystems can further comprise a global position system (GPS) 107. A GPS can be used as a navigation aid, for example for leading the vehicle to a certain target destination. However, it is noted that according to the presently disclosed subject matter a GPS may not be necessary for operation of the system and vehicle.

System 100 can further comprise one or more computer storage devices 109 for storing information such as one or more maps, information on obstacles, navigation commands, or the like. System 100 can further comprise one or more communication units 111 for facilitating communication with the vehicle.

System 100 can comprise, by way of example, navigation computer 120 configured to execute various operations related to navigation of the vehicle including the generation of navigation instructions. Navigation computer 120 can comprise or be otherwise operatively connected to one or more processing circuitries for controlling and executing various operations, as disclosed herein. Each processing circuitry comprises at least one computer processor which can be, for example, operatively connected to a computer-readable storage device having computer instructions stored thereon to be executed by the computer processor. For illustrative purposes and simplicity Fig. 1 shows a single processing circuitry comprising functional modules. The specific division into functional modules is provided by way of example and should not be construed as limiting.

Navigation computer 120 can comprise, by way of example, map generation module 121, localization module 121, path planning module 123 and vehicle navigation unit 130.

Map generation module is configured to execute a terrain mapping process. Map generation module 121 is configured to receive and process readings from scanning device 101 and generate and/or update one or more maps representing the area surrounding the autonomous vehicle. Map generation module 121 can further make use of INS data during map generation. For example, the generated map can be accumulative, where new scanning output data is continuously being added to the existing data, such that the relative map combines scanning output data from a plurality of scanning operations. As the vehicle proceeds, new areas are added to the map, and areas that reach the end of the map are excluded from further map updates. INS data indicative of changes in the position of the vehicle over time can be used for aligning between scanning output data from different (e.g., consecutive) scanning operations.

Localization module 122 is configured to determine the relative and/or absolute position of the vehicle. Absolute position is maintained with respect to a global reference frame (e.g., Latitude Longitude Attitude) and can be determined, for example, based on GPS data or based on INS data, which is aligned with a global reference frame. Relative localization is maintained relative to the specific environment in which the vehicle is traveling, where, according to examples of the presently disclosed subject matter, INS is used for tracking relative position of the vehicle within the area of the map.

As disclosed in PCT Patent Application PCT/IL2018/050966 (entitled "Method of Navigating an Unmanned Vehicle and System Thereof", No. PCT/IL2018/050966, filed August 30, 2018, by the Applicant, and which is incorporated herein by reference in its entirety), in some examples, the vehicle is tracked using a map of the environment and an inertial navigation system in conjunction with real-time scanning. As mentioned above, the map is generated and repeatedly updated by scanning the area surrounding the vehicle using one or more scanning devices operated from onboard the vehicle (e.g., with the help of map generation unit 121). The resulting map is thus generated relative to the vehicle and represents an area surrounding the vehicle (sometimes referred to herein as a "relative map"). The map can comprise a grid of cells, wherein each cell represents part of the mapped area. Between successive updates of the map which can be performed according to readings received from the scanning device, the vehicle advances, and its location can be tracked based on readings received from an inertial navigation system (INS), e.g., with the help of map localization module 122. Thus, the vehicle continuously navigates through the traversed area and avoids obstacles by repeatedly scanning the area surrounding the vehicle, generating a relative map of the area based on the scanning output data, and using an INS for updating the position of the vehicle, and navigating the vehicle according to the map.

According to some examples system 100 implements a method that includes: operating the scanning device for repeatedly executing a scanning operation, where each scanning operation includes scanning an area surrounding the vehicle, to thereby generate respective scanning output data; operating the computer for generating, based on the scanning output data from multiple scanning operations, a relative map representing at least a part of the area, the relative map having known dimensions and being relative to a position of the vehicle, wherein the relative map comprises cells, each cell being classified to a class selected from at least two classes, comprising traversable and non-traversable, and characterized by dimensions equal to or larger than an accumulated drift value of the INS over a predefined distance; wherein non- traversable cells correspond to obstacles identified in the scanning output data; receiving INS data and updating a position of the vehicle relative to the cells based on the INS data. The predefined distance can be derived for example, from the known dimensions of the map. For example, it can be equal to (or greater than) a length of a map edge or half the length of the map edge, or equal to (or greater than) a diagonal extending between two opposite map vertices, or some other distance derived from the map dimensions.

The fact that an entire cell is classified as non-traversable irrespective of the actual size of the obstacle(s) located within the area of the cell, together with the fact that the size of the cell is greater than a maximal accumulated INS drift over a given distance, protects from collision with obstacles which may result from INS drift over the given distance. The accumulated drift over the given distance can induce a navigation error which is smaller than the size of a cell, but not greater. Thus, because of such error, the UGV may deviate from its actual location only within the boundaries of a cell, thus setting an error limit which is equal to the size of a cell. Since a traversable cell is completely clear of obstacles, deviation within the boundaries of a cell does not pose a risk of collision with an obstacle. This method allows to navigate within the area of the map using INS alone, without GPS, while the drift induced error of the INS is restricted by an upper limit defined according to the cell size.

Path planning module 123 is configured in general to determine the path along which the vehicle should advance. Path planning module 123 is configured to generate, or obtain from some other source, a path leading the vehicle to a target destination. In some examples, path planning module 123 can be configured to obtain data indicative of a destination to which the UGV has to arrive. To this end path planning module 123 can be operatively connected to communication unit 111 for receiving data including the path and/or the destination from some external source. System 100 can further comprise various devices that can be utilized for identifying a destination. For example, GPS 107 can be used for navigating the vehicle to the target destination according to its global coordinates. In other examples, system 100 can comprise a computer vision system (not shown) comprising one or more cameras operatively connected to a processing circuitry configured to capture images of the environment, process the images, and identify in the images the target destination, to enable the vehicle to advance towards the destination. Assuming, for example, data is provided to the vehicle indicating that the target destination is marked by a distinguishable mark (e.g., light in the visible or infrared spectrum, a specific structure or object, a flag, a colored object, or surface, etc.,) the computer vision system can be configured to identify the mark and generate navigation instructions indicating the direction the vehicle should travel to reach the destination.

A path that safely and efficiently leads the vehicle to the target destination is desired. Such a path would strive to avoid obstacles or other areas which are undesired, and comply with additional limitations such as pitch and roll angle limitation, maintaining the path as short as possible, etc.

According to some examples, a navigation technique is applied, where course path planning towards a target destination is carried out using GPS, or GPS combined with INS or some other alternative means, and fine path planning within the area of the map for avoiding obstacles is carried out using the relative map and INS (without GPS), as explained with respect to Figs. 6a, 6b and 7 in PCT/IL2018/050966. A global path leading to a destination may comprise a plurality of way points. To enable navigation of the vehicle based on the global path, while avoiding obstacles according to the relative map, the global map and relative map can be aligned. For example, system 100 can be configured to receive GPS data indicative of the position of the vehicle in absolute coordinates indicating its position relative to a global path leading to a desired destination, and the relative map indicating the position of obstacles relative to the vehicle, are aligned one with the other effectively, causing the navigation instruction generated based on the relative map to update the global path, and allowing the vehicle to proceed towards the destination while avoiding obstacles.

System 100 is further configured to generate navigation instructions dedicated for leading the vehicle to a desired destination. For example, a navigation computer can comprise command generation module 131, configured to generate navigation instructions. The navigation computer 120 is also operatively connected to vehicle control sub-systems 150 including for example steering subsystem 151, gear subsystem 152, and speed control subsystem 153, which further includes breaking subsystem and throttle, etc. Navigation instructions are provided to vehicle control sub-systems 150 and translated into vehicle control commands (e.g., steering commands, acceleration deceleration commands, etc.) which are executed for controlling the AV accordingly. Such commands include, by way of example, "gas 30%, yaw rate 50%", "turn right", "brake", or the like.

In some vehicles (e.g., some heavy-duty autonomous ground vehicles), a delay (referred to herein as "command execution delay") is known to exist between the time at which a steering command is received by the vehicle control systems, and the time at which the command is executed. For example, in some cases, a delay between 200 milliseconds (msec) and 750 msec is observed. Command execution delay may result, for example, from mechanical latencies and/or processing latencies in different assemblies of the vehicle, such as the internal communication system or other electrical or electronic assemblies. Thus, generating a vehicle control command (e.g., steering command) based on a current location of the vehicle, while ignoring the expected delay, may lead to undesired results, such as collision with obstacles or deviations from a designated path. This is so, since the vehicle continues to move during the delay period, from its current location according to its current velocity and acceleration, before the current command is executed.

Additionally, previous commands which are pending and are bound to be executed prior to execution of a current (more recent) command, may also change the location and direction of the vehicle at the time of execution of the current command. For example, a new command may be issued in response to received data, indicating that a trigger event has occurred. This includes a timer event when a commands execution is coordinated with time. A timer event can be received for example, every 100 msec, while the command execution delay is 500 msec, thus, according to this example, five commands may be executed between the time a command is issued and the time it is executed. Therefore, the change in vehicle location, heading and speed during the command execution delay depends on the accumulative change caused by all commands which are executed during the period of the delay. Thus, to guide the vehicle safely and efficiently, it is required to consider the effect of such delays, and specifically pending commands.

System 100 can be configured to consider these delays when generating the navigation commands. To this end system 100 can comprise, for example, a vehicle state prediction model 135 configured to operate in conjunction with command generation module 131 and calculate a predicted state of the vehicle following the execution time delay and the execution of all pending commands. When a new command is generated, it is done while considering the expected or predicted state of the vehicle following the command execution delay, i.e., at the time the command is executed. In some examples, prior to generating a navigation command, the future position, orientation and velocity of the vehicle at the (predicted) execution time of the navigation command is estimated, where the expected state is estimated based on accumulated effects of execution of one or more pending commands preceding the new navigation command and which are ready to be executed during a delay in execution of the new navigation command, such that the new command is generated in accordance with the estimated position, orientation and velocity of the vehicle at the time of the command execution. Vehicle parameters such as position, orientation and velocity are collectively referred to herein as "state" or "vehicle state". Notably, in some examples, additional parameters such as acceleration, may also be considered within the vehicle state. In some examples, the term "vehicle state" is interpreted to include one or more of the above vehicle parameters (e.g., including at least position), in some examples the term "vehicle state" is interpreted to include two or more the above vehicle parameters (e.g., including at least position and velocity) and in further examples term "vehicle state" is interpreted to include three or more of the above vehicle parameters (e.g., including at least position, velocity, and orientation). Given a navigation command, to estimate the future state of the vehicle at the time of its execution, the integrated effect of all pending commands bound to be executed during the command's execution delay on the vehicle state, is assessed. Such assessment can be done by processing the commands in the order in which they are received, and, for each command, estimating the effect it will have on the state of the vehicle after the previous command has been executed. Following execution of the pending commands, the new command is executed for controlling the vehicle. Thus, as part of the calculation of a new navigation command, the navigation computer (e.g., command generation module 131) uses the predicted state of the vehicle following execution of all pending commands. A more detailed description of the navigation commands generation, while taking into consideration command execution delay, is described in PCT patent application no WO2019/215726 filed May 6, 2019 by the Applicant, and particularly as explained in Figs. 3A, 3B, 4A and 4B and the associated description on page 14 line 19 to page 19 line 25 of WO2019/215726 , which is incorporated herein by reference in its entirety.

According to the presently disclosed subject matter, navigation computer 120 is further configured (e.g., by a stability control module 133) to detect vehicle instability. As further explained below with reference to the following figures, in case instability is detected, a speed control command is generated for maintaining the speed under a certain threshold value.

Reference is now made to Fig. 3A showing a flowchart of operations carried out as part of vehicle speed control, according to some examples of the presently disclosed subject matter. Operations described with reference to Fig. 3A are described by way of example with reference to components of the system shown in Fig. 1, including for example, vehicle navigation computer 120 and specifically stability control module 133, however this should not be construed to limit the operations to any specific system design or component.

In some examples the stability control module operates as a feedback control loop, where the speed of the vehicle is adapted according to the determined stability of the vehicle.

At block 301 a future vehicle state is determined. As mentioned above, a vehicle state can be defined by various navigation parameters such as its position, speed and heading (velocity state vector), etc. The future (or predicted) vehicle state pertains to a vehicle state which is anticipated in the future, namely following a certain time-window. For example, assuming two time points Ti and T2 separated by a timewindow (TW) of 500-700 milliseconds, at Ti a prediction is made, predicting what the future vehicle state will be at time T2.

The future vehicle state can be determined based on a current vehicle state (at Ti), which includes navigation parameters such as its current position, speed and heading, which enable to calculate an anticipated displacement (including future position and orientation) of the vehicle following the time-window. In some cases, where the vehicle exhibits command execution delays as described above, pending commands, which are awaiting execution and are bound to be executed during the time-window, are also considered as part of the prediction of the future vehicle state. Vehicle state can be predicted, for example, with the help of vehicle state prediction module 135.

The current vehicle state (specifically, but not exclusively, its current position) which is determined at time Ti represents the basic navigation parameters, and the future vehicle state represents the predicted navigation parameters at time T2. Using GPS for tracking the position of the vehicle within the traverse area is inadequate for accurate navigation of the vehicle, due to inaccuracies resulting from jitters and delayed updates, which result in jumps in the obtained vehicle state. Therefore, according to the presently disclosed subject matter, a self-contained navigation system (e.g., INS 103) onboard the vehicle is used, rather than a GPS, by combining a previously known state and information received from the INS indicating changes in the navigation parameters (for example, position can be determined based on acceleration measurements' integration and heading). It is well known that tracking the position of the vehicle using SCNS (such as an INS) alone, may account for significant errors resulting from drift that accumulates as the vehicle advances. However, as the difference between a current vehicle state and a future vehicle state is important (rather then for example the absolute position of the vehicle), the INS drift-induced error is limited by the size of the time-window and does not accumulate from one window to the next. By choosing a short time-window, the drift-induced error accumulated during the period of the time-window is maintained at a low value and does not result in significant inaccuracies in the vehicle state calculation. Accordingly, in some examples the SCNS drift can be ignored. As the drift is dependent on the distance travelled during the time-window, the drift in a slow-moving vehicle which covers a smaller distance is smaller than the drift in a fast-moving vehicle, therefore the time-window can be also adapted according to the traveling speed of the vehicle (e.g., maximal travelling speed or average travelling speed), where a slow- moving vehicle can use a longer time-window than a fast-moving vehicle.

At block 305 the actual vehicle state following the time-window (being a new current vehicle state at time T2) is determined. This is done using INS data for determining the relative position of the vehicle at time T2. Unlike the predicted vehicle state, the actual vehicle state is determined based on the actual measured IMU values, and not based on a prediction.

At block 307 the predicted vehicle state is compared with the actual vehicle state. This includes, for example, comparing between the actual vehicle displacement determined between the vehicle state at time Ti and the vehicle state at time T? ,and the predicted vehicle displacement determined between the vehicle state at time Ti and the future vehicle state, predicted for time T2.

If the difference between the predicted (or expected) vehicle state and the actual vehicle state is greater than a certain threshold (block 309), this serves to indicate vehicle instability, resulting for example from loss of traction. As a result, a need to limit the speed of the vehicle to a certain maximal speed threshold, allowed under instability conditions, is determined, and an "instability speed limitation" is applied, limiting the allowed maximal speed according to the maximal speed threshold. If the speed of the vehicle is greater than the maximal threshold (block 311), a vehicle control command (e.g., speed control command) is generated for reducing the speed of the vehicle, so it is below the maximal threshold. To this end, stability control unit 133 can be configured to receive feedback from the speed control subsystem 153 indicative of the current vehicle speed. The control command is transmitted to the vehicle control subsystem (e.g., speed control subsystem 153), which reduces the vehicle's speed in response to the command. According to some examples, the command is transmitted directly from stability control module 133 to speed control subsystem 153, where, in other examples the command can be transmitted by some other component of navigation computer 120. Notably, in some examples, only a warning may be generated, without a direct command for changing the vehicle speed. The warning can be displayed to an operator or driver, advising to reduce the speed (or increase the speed, as explained below with reference to Fig. 3B), where actual speed reduction is executed only following confirmation received from an operator.

If the difference between the predicted vehicle state and the actual vehicle state is smaller than (and in some examples also if it is equal to) the predefined threshold, or if the difference between the predicted vehicle state and the actual vehicle state is greater than the predefined threshold and the speed of the vehicle is below the maximal speed threshold, the process returns to block 301 where the next predicted vehicle state following the next time-window is calculated. In the following time-window (e.g., between T2 and T3), the current vehicle state (specifically but not exclusively, its current position) determined at time T2 represents the basic navigation parameters and the future vehicle state represents the predicted navigation parameters at time T3, which is calculated relative to T2. This is illustrated graphically in Fig. 2, which shows that each predicted vehicle state is determined relative to the previous vehicle state over a short time-window, during which the drift of the INS does not have a significant impact on the accuracy of the navigation parameters. Since each calculation is relative to the previous vehicle state, and is independent of any states preceding the previous vehicle state, the drift does not accumulate to cause significant inaccuracies.

The specific threshold value for determining a substantial difference between the predicted (or expected) vehicle state and the actual vehicle state depends on the desired safety threshold. A smaller threshold value provides a stricter safety requirement, whereas a greater threshold value provides a more lenient safety requirement. According to some examples of the presently disclosed subject matter the threshold value may be dynamically adapted by the system in real-time. For example, the threshold can be determined according to an estimated risk score to the vehicle, which may depend on the current area in which the vehicle is traveling. For instance, whether the vehicle is traveling in a safe area that poses little risk to the vehicle, or a dangerous area which poses a greater risk to the vehicle. A safe area can be, for example an open area with little obstacles (e.g., an open clear area, or a football field). A dangerous area can be, for example an area dense with obstacles, or a narrow path on the edge of a precipice, where a steering error may result in the vehicle being thrown over the ridge. The threshold value can be determined according to the estimated risk score of the area surrounding the vehicle, where a smaller threshold value is used in an area with higher risk, and a greater threshold (allowing a greater difference between the estimated and measured vehicle state) value is used in an area with low risk.

According to some examples, navigation computer 120 can be configured to process imaging data of the area being traversed, to determine an estimated risk score and set the threshold accordingly (e.g., by map generation module 121). In some examples, the imaging data of the area can be continuously obtained in real-time as the vehicle advances through the area, e.g., by a scanning device such as a camera or laser scanner fixed to the vehicle. This can be done for example as part of the processing of scanning output data received from the scanning device onboard the vehicle and identification of obstacles, which is performed for the purpose of navigating the vehicle through the area. In other examples it can be done using other imaging data of the traversed area obtained from some other source.

This can be a dynamic process where the threshold value is continuously updated according to the current characteristics of the area in which the vehicle is traveling. For example, navigation computer 120 can be configured to continuously process the scanning output data and determine the obstacle density (e.g., number of detected obstacles per square meter) in the scanned area, and determine the threshold accordingly, where a smaller threshold is determined in case of high obstacle density, and a greaterthreshold is determined in case of low obstacle density. Likewise, the scanning output data, as well as information received from other sources, such as maps of the area or databases, can be processed for determining the risk to the vehicle. In some examples, navigation computer 120 can be configured to align between a global map (in absolute coordinates) of the travelled area with the relative map generated in real-time, to determine characteristics of the area being traversed. If the maps processing output indicates that the vehicle is currently advancing in a dangerous area, e.g., along a ridge, or is descending a steep slope, the threshold value is reduced. If the maps processing output indicates that the vehicle is currently advancing in a safe area, e.g., along a straight paved road, the threshold value is increased. Alternatively, or additionally, navigation computer 120 can be operatively connected to a data source (e.g., a database) that stores risk score data, which includes a risk score assigned to different locations within the traversed area. This can be applicable, for example, where an area has been previously traversed, while assigning risk scores to different locations within the area according to the estimated risk in each location. The risk scores can be stored in the database, and once the same area is being traversed again, the risk scores can be provided to navigation computer 120 according to the absolute position of the vehicle. For example, the area can be divided into sub-sections of a certain size, each sub-section being assigned with a respective risk score. Each sub-section can be stored with the corresponding risk score. When the vehicle reaches a certain sub-section of the area, the respective risk score is pulled from the database and used for determining the instability threshold in real-time. In some examples, both methods can be applied, where the imaging data is being processed in real-time and used for enhancing the risk score which is stored in the database.

In some examples, the threshold value is adapted according to the speed of the vehicle, where at a greater speed the risk to the vehicle is increased (e.g., collision risk) and accordingly a stricter threshold is used and in a lower speed the risk to the vehicle is reduced and accordingly the threshold value is greater. The threshold value can be dynamically updated by the system according to the current speed of the vehicle. In some examples, the threshold value is calculated based on a compilation of parameters such as the obstacle density and the speed. In some examples, each parameter is assigned with a score and weight, the score indicating the severity (e.g., obstacle density score and speed) and weight indicates the contribution of each parameter in the determination of the threshold.

While it has been explained that due to the small time-widows, the SCNS drift can be ignored when determining instability, in some example, when setting the threshold value, the actual drift of the INS is considered to ensure that the calculated difference between estimated and actual position is not affected by the INS drift. This can be done for example, by adding the drift to a basic threshold value. According to one example, a certain drift value can be determined and used for setting the threshold value (e.g., maximal drift value calculated based on the nominal drift of the INS, the period of the time-window, and the maximal speed of the vehicle). According to another example, the observed drift can be calculated in real-time based on the nominal drift value and the distance traveled during the time-window (thus, in effect taking the speed of the vehicle into consideration when determining the threshold). In such cases, the value of the threshold can be dynamically adapted in real-time according to the observed drift, by adding a basic threshold value to the observed drift.

It is noted that the disclosed technique allows to determine instability while disregarding the specific conditions of the area in which the vehicle is travelling, e.g., whether the surface is wet, muddy, sandy, covered with snow, paved, unpaved, covered with gravel, and so forth. This is so since the performance of the vehicle and its respective stability/instability is directly derived from these conditions, and therefore they are inherently considered in the calculation, and there is no need to consider them independently.

It is further noted that, in some examples, to avoid false instability detection as a result of momentary instabilities or miscalculations, or to increase the accuracy of instability detection in general, a decision as to whether vehicle instability exists is made based on more than one time-window, for example the operations described with reference to blocks 301-309 are repeated for 2, 3, 4 or 5 (or more) consecutive time-windows, and vehicle instability is determined in case it persists in all (or most, or at least x out of y) of the time-windows. For example, instability determination can be implemented for each one of the time-windows illustrated in Fig. 2 where a speed control command is generated at the end of the fourth window based on the collective findings from all the four preceding windows. By using discrete time-windows with a size that allows for the accumulation of a small INS drift-induced error (below than some predefined value), it is possible to use an INS for monitoring vehicle operation and detecting instability, while avoiding significant inaccuracies resulting from INS drift.

The operations according to Fig. 3A can be continuously repeated as the vehicle advances, thus, assuming the vehicle is advancing according to some speed determined by the navigation computer (e.g., a maximal allowed speed under normal (stable) conditions), the presently disclosed subject matter provides a stability control module that operates as a feedback control loop, configured to continuously and autonomously monitor vehicle performance and identify instability conditions, and automatically generate control commands for adapting the speed of the vehicle accordingly.

Attention is now reverted to Fig. 3B showing another flowchart of operations carried out as part of vehicle speed control, according to some examples of the presently disclosed subject matter. Fig. 3B includes, in addition to operations 301 to 313 described above with reference to Fig. 3A, additional operations related to increasing the speed of the vehicle, thus implementing a closed control loop. Consider for example, that vehicle instability has been identified, and, as a result, the vehicle speed has been limited to the maximal speed threshold allowed under instability conditions. In such cases, the system continues to monitor instability, and in case instability is no longer detected ("No" in block 309), the limit on the instability speed limitation is removed, allowing the vehicle to increase its speed. For example, this could occur if the vehicle has advanced to more stable ground, e.g., has advanced from muddy ground to paved road.

In some examples, in case in vehicle instability is not detected, it is further determined whether the speed of the vehicle is limited (block 315), and, if so, a vehicle control command (e.g., speed control command) is generated for releasing the limitation, enabling to increase the speed of the vehicle according to a currently desired speed (e.g., up to a maximal allowed speed). The command is transmitted to the vehicle control system (e.g., speed control subsystem 153), which can increase the vehicle's speed in response to the command (block 317). According to some examples, the instability speed limitation is not removed at once to immediately bring the speed of the vehicle to the maximal speed. Instead, the speed is increased incrementally, where a command for increasing the speed of the vehicle by an incremental value is generated, the incremental value being a fraction of the difference between the current speed and the desired speed e.g., maximal allowed speed. After each incremental increase, it is determined whether vehicle instability is observed. In case vehicle instability is not detected, the speed is increased further, until the desired speed is reached. If, however, vehicle instability is detected, the speed is decreased, as explained above. Increments can be constant (e.g., 5 KMP each time), or variable. For example, the increment can increase or decrease as the overall increase gets closer to the maximal allowed speed.

It is noted that the teachings of the presently disclosed subject matter are not limited to unmanned ground vehicles alone, and can be likewise implemented in other types of vehicles such as aerial, marine vehicles, and submarine vehicles. In such vehicles, the deviation of the vehicle from an expected vehicle state can be caused by reasons other than loss of traction (e.g., winds, in the case of aerial and marine vehicles, or tides or currents in the case of marine vehicles).

It is noted that while the description herein mainly refers to an autonomous vehicle, the principles and operations disclosed herein can be likewise applied in a human operated vehicle. For example, where a human operator (e.g., driver or pilot) is controlling (e.g., driving) the vehicle, and a system as disclosed herein is installed in the vehicle. The disclosed system can comprise, for example, a warning module comprising a circuitry that is configured to issue a warning to the operator in case instability conditions are detected, advising the driver to reduce the vehicle's speed. In other examples, a human controlled vehicle can be equipped with an automatic speed control subsystem operatively connected to the vehicle's speed control. The automatic speed control subsystem can be configured, when engaged and operable, to automatically control the speed of the vehicle according to the stability of the vehicle, as disclosed herein above with reference to Figs. 3A and 3B. In some examples, speed control by the automatic speed control subsystem overrides the human operator's control over the vehicle, while in other examples the human operator may override the automatic speed control subsystem. In an operator- controlled vehicle, some of the components of the system as disclosed herein with respect to the figures, may not be present. For example, the vehicle may not include a navigation computer as described with reference to Fig. 1, but only part of its components, e.g., module 133 and 135.

Those skilled in the art will readily appreciate that various modifications and changes can be applied to the examples of the invention as hereinbefore described without departing from its scope, defined in and by the appended claims.

Claims

1. A method of speed-control over a vehicle comprising a self-contained navigation system (SCNS), the method comprising: while the vehicle is advancing through an area: i. determining a first current vehicle state based on SCNS data obtained from the SCNS, the first current vehicle state including at least a first current vehicle position; ii. predicting, based on the first current vehicle state, a first future vehicle state following a first time-window; wherein the first future vehicle state comprises at least a first future vehicle position relative to the first current vehicle position; following the first time-window: iii. determining a second current vehicle state based on SCNS data obtained from the SCNS, the second current vehicle state including at least a second current vehicle position relative to the first current vehicle position; and iv. if a difference between the first future vehicle position and the second current vehicle position is greater than an instability threshold value, detecting vehicle instability; and v. generating a command for applying speed limitation that limits the vehicle's speed to a maximal speed threshold allowed under instability conditions.

2. The method of claim 1 further comprising: if the speed of the vehicle is greater than the maximal speed threshold, generating a command to a speed control subsystem for reducing the speed of the vehicle.

3. The method of any one of the preceding claims, further comprising: if a difference between the first future vehicle position and the second current vehicle position is lower than the instability threshold value, detecting vehicle stability; and if speed limitation is currently applied, generating a command for removing speed limitation and allowing to increase the vehicle's speed.

4. The method of claim 3 further comprising: if the speed of the vehicle is lower than the maximal threshold, generating a command to a speed control subsystem for increasing the speed of the vehicle.

5. The method of claim 4 further comprising: generating incremental commands, each incremental command dedicated to increasing the speed of the vehicle by an incremental value.

6. The method of claim 5, further comprising: following execution of each incremental command, repeating operations i-iii; if a difference between the first future vehicle position and the second current vehicle position is lower than the instability threshold value, generating a further incremental command dedicated for further increasing the speed of the vehicle by an incremental value; otherwise, applying speed limitation that limits the vehicle's speed to a maximal speed threshold allowed under instability conditions.

7. The method of any one of the preceding claims, wherein the instability threshold is determined according to an estimated risk to the vehicle.

8. The method of any one of the preceding claims, further comprising: processing imaging data of an area surrounding the vehicle, and determining a risk score indicating a level risk to the vehicle while traversing the area, and adapting the instability threshold value according to the risk score.

9. The method of claim 8 wherein the processing comprises: as the vehicle is advancing through the area, operating a scanning device located onboard the vehicle for capturing the imaging data.

10. The method of any one of claims 8 and 9 further comprising processing the imaging data and determining obstacle density, and dynamically adapting in realtime the instability threshold value according to the obstacle density.

11. The method according to any one of the preceding claims, further comprising: repeating operations i-iv for a plurality of consecutive time-windows; and performing operation v if a portion of time-windows, out of the plurality of time-windows in which vehicle instability is detected, is greater than a certain value.

12. The method of any one of the preceding claims, wherein the vehicle exhibits a delay in execution of navigation commands generated for navigating the vehicle, wherein the first future vehicle state is predicted based on accumulated effects of execution of one or more navigation commands pending execution during the first time-window.

13. The method of any one of the preceding claims, wherein the first current vehicle position, the first future vehicle position, and the second current vehicle position, are determined in a relative coordinate system.

14. The method of any one of the preceding claims, wherein the vehicle is an autonomous vehicle that travels autonomously in the area.

15. The method of any one of the preceding claims, wherein the SCNS is an inertial navigation system.

16. A system mountable on a vehicle for providing automatic speed control over the vehicle, the system comprising a self-contained navigation system (SCNS) and at least one processing circuitry configured to: determine a first current vehicle state based on SCNS data obtained from the SCNS, the first current vehicle state including at least a first current vehicle position; predict, based on the first current vehicle state, a first future vehicle state following a first time-window; wherein the first future vehicle state comprises at least a first future vehicle position relative to the first current vehicle position; following the first time-window: determine a second current vehicle state based on SCNS data obtained from the SCNS, the second current vehicle state including at least a second current vehicle position relative to the first current vehicle position; and detect vehicle instability if a difference between the first future vehicle position and the second current vehicle position isgreaterthan an instability threshold value; and generate a command for applying speed limitation that limits the vehicle's speed to a maximal speed threshold allowed under instability conditions.

17. The system of claim 16, wherein the at least one processing circuitry is configured, in case the speed of the vehicle is greater than the maximal speed threshold, to generate a command to a speed control subsystem for reducing the speed of the vehicle.

18. The system of any one of claims 16 to 17, wherein the at least one processing circuitry is configured to: detect stability if a difference between the first future vehicle position and the second current vehicle position is lower than the instability threshold value; and if speed limitation is currently applied, generate a command for removing speed limitation and allowing to increase the vehicle's speed.

19. The system of claim 18, wherein the at least one processing circuitry is configured to generate a command to a speed control subsystem for increasing the speed of the vehicle if the speed of the vehicle is lower than the maximal threshold, and generating a command to a speed control subsystem for increasing the speed of the vehicle.

20. The system of claim 19, wherein the at least one processing circuitry is configured to generate incremental commands, each incremental command dedicated to increasing the speed of the vehicle by an incremental value.

21. The system of claim 20, wherein the at least one processing circuitry is configured, following execution of each incremental command, to repeat execution of operations i-iii; and generate a further incremental command dedicated to further increasing the speed of the vehicle by an incremental value if a difference between the first future vehicle position and the second current vehicle position is lower than the instability threshold value; and otherwise, apply speed limitation that limits the vehicle's speed to a maximal speed threshold allowed under instability conditions.

22. The system of any one of claim 16 to 21, wherein the instability threshold is determined according to an estimated risk to the vehicle.

23. The system of claim 22, wherein the at least one processing circuitry is configured to process imaging data of an area surrounding the vehicle, determine a level risk to the vehicle while traversing the area, and adapt the instability threshold value according to the risk score.

24. The system of claim 23, wherein the at least one processing circuitry is configured, while the vehicle is advancing through the area, to operate a scanning device located onboard the vehicle for capturing the imaging data.

25. The system of any one of claims 23 to 24, wherein the processing circuitry is configured to process the imaging data, determine obstacle density, and dynamically adapt in real-time the instability threshold value according to the obstacle density.

26. The system of any one of claims 16 to 25, wherein the processing circuitry is configured to repeat operations i-iv for a plurality of consecutive timewindows; and perform operation v if a portion of time-windows out of the plurality of time-windows in which vehicle instability is detected, is greater than a certain value.

27. The system of any one of claims 16 to 26, wherein the vehicle exhibits a delay in execution of navigation commands generated for navigating the vehicle, wherein the first future vehicle state is predicted based on accumulated effects of execution of one or more navigation commands pending execution during the first time-window.

28. The system according to any one of claims 16 to 27, wherein the SCNS is an inertial navigation system.

29. A vehicle comprising the system according to any one of claims 16 to 28.

30. The vehicle of claim 29 is an autonomous vehicle.

31. The vehicle of claim 30 is an unmanned ground vehicle.

32. A computer program product comprising a computer readable storage medium retaining program instructions, which programs instructions when read by a processor, and causes the processor to perform a method according to any one of claims 1 to 16.