WO2023128859A1

WO2023128859A1 - Uwb-based system and method for coordinated movement of vehicles in a vehicle convoy

Info

Publication number: WO2023128859A1
Application number: PCT/SG2021/050831
Authority: WO
Inventors: Nathanael En Ze TAN; Hua Chong LIM; Yong Way CHEE
Original assignee: St Engineering Land Systems Ltd
Priority date: 2021-12-31
Filing date: 2021-12-31
Publication date: 2023-07-06

Abstract

The invention relates to a system and a method for coordinating the movement of vehicles in a convoy. An operator can set up the vehicles into a convoy by identifying a lead vehicle (LV) and one or more follower vehicles (FV's). The first vehicle acts as the leader while each of the other vehicles follows a vehicle in front of it. Each vehicle is equipped with an Inertial Navigation System and a set of Ultra-Wide Band radio nodes through which the vehicles can communicate with each other to provide vehicle measurements and obtain the LV's range and bearing relative to the FV's. Using a Computer Software Configuration Item, relative range and bearings of the vehicles can be estimated as well as the trajectory of the LV's such that FV's follow their respective LV.

Description

UWB-BASED SYSTEM AND METHOD FOR COORDINATED MOVEMENT OF VEHICLES IN A VEHICLE CONVOY

TECHNICAL FIELD

[0001] The invention relates to convoy navigation, and more specifically, to a method and system for operating a plurality of vehicles in a coordinated manner using data from UWB radio nodes and inertial navigation systems.

BACKGROUND

[0002] A convoy is a group of vehicles, typically motor vehicles, traveling together for mutual support and/or protection. Conventional vehicle convoys use a human operator to drive each vehicle. The operator ensures that each vehicle maintains a proper speed and distance from other vehicles in the convoy while following a uniformed route traversed by the lead vehicle. Recent advances have focused on alternatives to manually operating each vehicle. Unmanned leader-follower convoys have been devised with semi- autonomous vehicle capability which allows unmanned followers (UF) to follow in the path of a man-driven lead vehicle (LV). This capability is intended to reduce the quantity and/or type of manpower required to operate a convoy of vehicles.

[0003] For example, WO2019240664A1 describes a system for navigating a convoy wherein a follower vehicle uses a camera to track a marker on a lead vehicle. The system allows a lead vehicle to be driven while other vehicles follow from a set distance. Because each vehicle relies on a light signal to detect a lead vehicle, a direct line of sight between must be maintained between each vehicle in the convoy. Obstacles can disrupt the signal and the signal can also be lost if a lead vehicle traverses around a corner. Because of this, following mobile robots will not necessarily follow the path of the manned LV as it turns or moves along a curve/arc. [0004] Other convoy systems combine technologies to enable the operation of an unmanned leader-follower convoy. For example, U.S. Patent No. 8,139,109 describes a vision system that uses a set of perception sensors (e.g., RADAR, LiDAR and cameras) to classify perceived objects around the UF before the LV is identified to be followed. Other implementations utilise GPS way points and telemetry information transmitted between vehicles for the UF to replicate the LV’s position and actions. The use of either approach incurs high component and computational costs, requires significant modifications to host vehicles and is susceptible to perception noise or poor GPS signals. A hybrid of the two systems also exists, where the accuracy of GPS way points is augmented by perception data. While promising, this method is still fundamentally susceptible to the aforementioned limitations.

[0005] Unfortunately, most indoor environments and many parts of urban areas lack reliable GPS reception. Accordingly, there is a need to provide an improved system for coordinating and operating groups of vehicles in a convoy. Such a system should provide a solution to the problems related to high cost, poor applicability and inoperability in GPS- denied environments that are associated with conventional leader-follower convoy technologies.

SUMMARY OF THE INVENTION

[0006] The following summary is provided to facilitate an understanding of some of the innovative features unique to the disclosed embodiments and is not intended to be a full description. A full appreciation of the various aspects of the embodiments disclosed herein can be gained by taking into consideration the entire specification, claims, drawings, and abstract as a whole.

[0007] In one embodiment, there is provided a system for coordinated movement of a plurality of vehicles, wherein a lead vehicle is followed by a follower vehicle to form a first lead-follower pair, said system comprising: a lead vehicle comprising a first set of Ultra-Wide Band (UWB) radio nodes, a first inertial navigation system to provide lead vehicle measures; a follower vehicle comprising a second set of Ultra-Wide Band (UWB) radio nodes, a second inertial navigation system to provide follower vehicle measures, and a navigation computer configured to execute one or more computer software configuration items (CSCI) for estimating the trajectory of the lead vehicle and producing a trajectory for the follower vehicle based on the lead and follower vehicle measures.

[0008] In one embodiment, the lead vehicle can further comprise a datalink and an Operator Control Unit (OCU) for user interface and communication of data.

[0009] In one embodiment, the follower vehicle can further comprise a datalink for communication of data.

[0010] In one embodiment, the lead vehicle is human-operated and the follower vehicle follows the path of the lead vehicle using the system described herein. In one embodiment of the system, the lead vehicle is manned and the follower vehicle is unmanned. In other embodiments, both lead and follower vehicles are unmanned.

[0011 ] In one embodiment, the system uses two algorithm modules to plan a desired trajectory to follow: (a) an ultra-wide band (UWB) computer software configuration item (CSCI) and (b) a leader-track module (LTM) computer software configuration item (CSCI).

[0012] In one embodiment, the system uses three algorithm modules to plan a desired trajectory to follow: (a) an ultra-wide band computer software configuration item (UWB CSCI); (b) a leader-track module computer software configuration item (LTM CSCI); and (c) a Pose Fusion computer software configuration item (CSCI).

[0013] In one embodiment of the system, the first set of UWB radio nodes comprises at least two UWB beacons. [0014] In one embodiment of the system, the second set of UWB radio nodes comprises at least three UWB nodes, wherein the UWB nodes are configured to obtain range measurements sequentially or non-sequentially with the UWB beacons.

[0015] In one embodiment of the system, the UWB nodes are connected to the navigation computer to trigger ranging.

[0016] In one embodiment of the system, the first and second set of UWB radio nodes perform two-way time of flight measurements to obtain the lead vehicle’s range and bearing relative to the follower vehicle.

[0017] In one embodiment of the system, the lead vehicle further comprises a first odometry information source connected to the first inertial navigation system, and the follower vehicle comprises a second odometry information source connected to the second inertial navigation system.

[0018] In one embodiment of the system, the CSCIs comprise a UWB CSCI to provide UWB states, a LTM CSCI to provide an estimate of the lead vehicle trajectory and a Pose Fusion CSCI to provide pose readings that are input into the LTM CSCI.

[0019] In one embodiment of the system, the UWB CSCI comprises a tracking filter to track the dynamic relative range and bearing of the lead vehicle.

[0020] In one embodiment of the system, the LTM CSCI comprises a probabilistic filter to estimate the lead vehicles trajectory based on the UWB states, the lead vehicle measure and the follower vehicle measure. The probabilistic filter can use Monte Carlo methods to estimate the lead vehicle’s trajectory based on the weighted average of a large number of samples. These samples are randomly generated and weighted via partial or complete observations of the relative vehicle measures or UWB states.

[0021] In one embodiment of the system, each follower vehicle can include a Pose Fusion CSCI to obtain it’s own pose readings, wherein a datalink and OCU are used to communicate the pose reading of the lead vehicle to the navigation computer in the follower vehicle. In one embodiment, the pose reading of the lead vehicle is not required, whereby only the vehicle measures are fed from the lead vehicle through a datalink to the LTM module. The intent of pose fusion is to obtain the lead vehicle’s pose readings, whereby the term “Pose FUSION” represents the ability to fuse multiple input sources to a single pose reading output. In one embodiment, pose fusion is also used on the lead vehicle to obtain its position. The lead vehicle’s position, however, is not used in raw form but used to derive vehicle measure.

[0022] In another embodiment, there is provided a method of coordinated movement of a plurality of vehicles wherein a lead vehicle is followed by a follower vehicle, said method comprising the steps of: creating a line of vehicles by positioning the follower vehicle behind the lead vehicle, wherein the lead vehicle comprises a first set of Ultra- Wide Band (UWB) radio nodes and a first inertial navigation system, and the follower vehicle comprises a second set of Ultra-Wide Band (UWB) radio nodes and a second inertial navigation system; driving the lead vehicle along an intended route; obtaining vehicle measures and the lead vehicle range and bearing relative to the follower vehicle using the first and second set of UWB radio nodes; obtaining an estimate of the trajectory of the lead vehicle using a navigation computer configured to execute one or more computer software configuration items (CSCI); and producing a trajectory for the follower vehicle to follow the lead vehicle.

[0023] In one embodiment of the method, the first set of UWB radio nodes comprises at least two UWB beacons. [0024] In one embodiment of the method, the second set of UWB radio nodes comprises at least three UWB nodes, wherein the UWB nodes obtain range measurements sequentially or non-sequentially with the UWB beacons.

[0025] In one embodiment of the method, the UWB nodes are connected to the navigation computer for triggering ranging.

[0026] In one embodiment of the method, the first and second set of UWB radio nodes perform two-way time of flight measurements to obtain the lead vehicle’s range and bearing relative to the follower vehicle.

[0027] In one embodiment of the method, the lead vehicle further comprises a first odometry information source connected to the first inertial navigation system, and the follower vehicle comprises a second odometry information source connected to the second inertial navigation system.

[0028] In one embodiment of the method, the CSCIs comprise a UWB CSCI to provide UWB states, a Leader-Track Module (LTM) CSCI to obtain an estimate of the lead vehicle trajectory and a Pose Fusion CSCI to provide pose readings that are input into the LTM CSCI.

[0029] In one embodiment of the method, the UWB CSCI comprises a tracking filter to track the dynamic relative range and bearing of the lead vehicle.

[0030] In one embodiment of the method, the LTM CSCI comprises a probabilistic filter to estimate the lead vehicles trajectory based on the UWB states, lead vehicle measurements and follower vehicle measurements.

[0031] In one embodiment of the method, the Pose Fusion CSCI obtains pose readings from the follower vehicle.

BRIEF DESCRIPTION OF FIGURES

[0032] The drawings described herein are for illustrative purposes only of selected embodiments and not all possible implementations, and are not intended to limit the scope of the present disclosure.

[0033] The summary above, as well as the following detailed description of illustrative embodiments, is better understood when read in conjunction with the appended drawings. For the purpose of illustrating the disclosure, exemplary constructions of the disclosure are shown in the drawings. However, the disclosure is not limited to specific methods and instrumentalities disclosed herein. Moreover, the drawings are not to scale.

[0034] FIG. 1 depicts a leader-follower pair of vehicles with respective hardware components according to an embodiment.

[0035] FIG. 2 depicts a convoy with two follower vehicles with respective hardware components according to an embodiment.

[0036] FIG. 3 depicts the Leader-Track Module Computer Software Configuration Item (LTM CSCI) and interrelationship with other modules.

[0037] FIG. 4 is a flow chart of the steps involved between the UWB CSCI, Pose Fusion CSCI and LTM CSCI in providing an estimate of lead vehicle trajectory.

DETAILED DESCRIPTION

Definitions

[0038] The terms used in this specification generally have their ordinary meanings in the art, within the context of the disclosure, and in the specific context where each term is used. Certain terms that are used to describe the disclosure are discussed below, or elsewhere in the specification, to provide additional guidance to the practitioner regarding the description of the disclosure. For convenience, certain terms may be highlighted, for example using italics and/or quotation marks. The use of highlighting has no influence on the scope and meaning of a term; the scope and meaning of a term is the same, in the same context, whether or not it is highlighted. It will be appreciated that the same thing can be said in more than one way.

[0039] Consequently, alternative language and synonyms may be used for any one or more of the terms discussed herein. Nor is any special significance to be placed upon whether or not a term is elaborated or discussed herein. Synonyms for certain terms are provided. A recital of one or more synonyms does not exclude the use of other synonyms. The use of examples anywhere in this specification including examples of any terms discussed herein is illustrative only, and is not intended to further limit the scope and meaning of the disclosure or of any exemplified term. Likewise, the disclosure is not limited to various embodiments given in this specification.

[0040] Without intent to further limit the scope of the disclosure, examples of instruments, apparatus, methods and their related results according to the embodiments of the present disclosure are given below. Note that titles or subtitles may be used in the examples for convenience of a reader, which in no way should limit the scope of the disclosure. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure pertains. In the case of conflict, the present document, including definitions, will control.

[0041] The term “computer software configuration item”, or “CSCI”, refers to a configuration item in software formed via the aggregation of software components into a software module that satisfies an end use function and is designated for separate configuration management by the acquirer.

[0042] The term “inertial navigation system” of “INS” refers to a sensor system comprising of, but not limited to, an “Inertial Measurement Unit”, or “IMU”, and a computational unit. Multi-degree of freedom measurements of acceleration, angular velocity and magnetic fields are computed to derive a sensor’s position and heading for the purposes of navigation.

[0043] The term “ultra-wideband,” “UWB” or “ultraband” refers to a radio technology that can use a broad radio frequency spectrum that allows for precise ranging via two way time of flight measurements. UWB has traditional applications in non-cooperative radar imaging. Recent applications target sensor data collection, precision locating and tracking applications.

[0044] The term “probabilistic filter” refers to a filter that can use Monte Carlo methods to estimate the lead vehicle’s trajectory based on the weighted average of a large number of samples. These samples are randomly generated and weighted via partial or complete observations of the relative vehicle measures or UWB states.

[0045] The term “tracking filter” refers to a filter that can be implemented to reject noise and improve accuracy and include a prediction step and an update step, where the filter uses the current relative range and bearing, calculates a prediction and associated error covariance of the preceding vehicle to be tracked, utilises measurements to check the accuracy of its predictions, then adjusts gains to improve the accuracy of the following prediction step to effectively track the preceding vehicle’s range and bearing estimates. The filter can be based off linear quadratic estimation and can include suitable transformations to handle system non-linearities. [0046] Other technical terms used herein have their ordinary meaning in the art that they are used, as exemplified by a variety of technical dictionaries. The particular values and configurations discussed in these non-limiting examples can be varied and are cited merely to illustrate at least one embodiment and are not intended to limit the scope thereof.

Description of Preferred Embodiments

[0047] Reference in this specification to "one embodiment/aspect" or "an embodiment/aspect" means that a particular feature, structure, or characteristic described in connection with the embodiment/aspect is included in at least one embodiment/aspect of the disclosure. The use of the phrase "in one embodiment/aspect" or "in another embodiment/aspect" in various places in the specification are not necessarily all referring to the same embodiment/aspect, nor are separate or alternative embodiments/aspects mutually exclusive of other embodiments/aspects. Moreover, various features are described which may be exhibited by some embodiments/aspects and not by others. Similarly, various requirements are described which may be requirements for some embodiments/aspects but not other embodiments/aspects. Embodiment and aspect can be in certain instances be used interchangeably.

[0048] Conventional, non-GPS leader-follower systems typically provide instantaneous relative positioning information from a lead vehicle to a following vehicle. This causes the following vehicle to follow a straight-line, point-to-point path and does not allow the follower vehicle to follow in the lead vehicle’s actual path. Other problems include physical obstacles, inadequate lighting and week signal transmission/reception.

[0049] The system disclosed herein serves as a novel and cost-effective leaderfollower solution that is operable under GPS-denied conditions and is more readily implemented on conventional vehicles. The system allows for leader-follower applications without the need for GPS, however, when implemented with GPS the operable performance can be further enhanced. Further, the system allows for leaderfollower applications without the need for sensors such a LIDAR or cameras.

[0050] Embodiments include a system and method of operating a convoy/caravan of vehicles. While a vehicle is designated as a ’’follower” or “lead” vehicle, it is understood that both vehicles can assume the follower and lead roles interchangeably. Accordingly, each vehicle is preferably equipped with the appropriate set of components to allow it to function as both a follower and a leader. Each vehicle can be equipped with motorized features that are common in the art (e.g., a motor, batteries, gauges, etc.). A controller can direct speed and movement of each vehicle. Further, each vehicle can include (a) Ultra-Wide Band (UWB) Radio Nodes and/or UWB beacons and (c) an Operator Control Unit (OCU) and/or a navigation computer; and (c) an inertial navigation system.

[0051] Accordingly, the system can be applied in a follow-me application, where for example, an unmanned follower vehicle, can follow in the path of a manned/unmanned leader vehicle without the need for GPS navigation. In particular, the system disclosed herein can operate so that the follower vehicle follows the exact path of the lead vehicle, instead of point-to-point following, at a desired speed and safe distance without the dependence on and use of any absolute localisation sensors such as GPS. In particular, the system disclosed herein can be used on any wheeled or tracked land vehicle platforms with either native or add-on drive-by-wire functionalities.

[0052] An operator can activate the follower vehicle to follow the path/trajectory of the lead vehicle. Likewise, a third or subsequent vehicle can be activated to follow the path/trajectory of the second vehicle. Similarly, a fourth and additional vehicles can be included to increase the size of the convoy. Each will follow the vehicle directly in front of it. It will be appreciated that as more vehicles are added a cumulative error can occur between the first lead vehicle and the last follower if the convoy is too long. This will increase cross track error, however, the acceptability of the cross track error can depend on the operational context. As such, in one embodiment, every vehicle can be fitted with both leader and follower hardware and all vehicles can be software reconfigured to perform either leader or follower roles.

[0053] As a lead vehicle, the controller can be manually controlled by an operator, follow a pre-determined path or use autonomous technology. In one embodiment, the controller of the lead vehicle can be unmanned and remotely controlled by an operator. In one embodiment, the lead vehicle can be a manually driven vehicle.

[0054] As a following vehicle, the controller can be directed autonomously by a navigation computer that uses data and readings from the location and movement of a lead vehicle. In one embodiment, the follower vehicle can be unmanned and remotely controlled by an operator. Accordingly, the follower vehicle can be termed herein as an unmanned follower (UF).

[0055] Additional components that are common in the art can be included so that a vehicle is capable of functioning as a leader and/or follower, as described further below.

[0056] In one embodiment, each lead and follower vehicles can include an Operator Control Unit (OCU) to allow operators to 1 ) Configure the convoy roles (leader/follower); 2) Monitor the speed, state and other details about each vehicle of the convoy; 3) Monitor each vehicle’s perceived surroundings and intended path of travel; and 4) Provide a simple debugging interface. In this regard, the OCU can provide computer-based controls over the convoy and include a user interface.

[0057] While the OCU can be in both the lead and follower vehicle, the OCU’s inclusion in the follower vehicle is not required in the absence of operator in the follower vehicle. In one embodiment, an OCU can be in the lead and the one or more follower vehicles, however only the lead vehicle’s OCU will run the Pose Fusion CSCI that can be managed by the navigation computer. [0058] In one embodiment, the OCU can include a pose fusion CSCI. In addition, the OCU can include modules that work to supervise the convoy movement and vehicle states to enable an operator to control the convoy via a user interface. This can run on whichever OCU is in control of the convoy while other OCUs in the system will only serve the function of relaying information to a user interface so that drivers have awareness.

[0059] The OCU and user interface can allow an operator to program each vehicle. For example, an operator can press a button to designate a lead vehicle. The operator can then assemble the follower vehicle and additional follower vehicles into a convoy behind the lead vehicle. The operator can then drive, direct or manually push the lead vehicle. In one embodiment, the OCU can allow the operator to remotely drive the lead and/or follower vehicles.

[0060] If a lead or follower vehicle does not have a suitable OCU to provide the required function, an OCU can be integrated into the controller of each vehicle.

[0061] In one embodiment, the lead vehicle can be manned and controlled by an operator, whereas the follower vehicle can be unmanned and operate autonomously.

[0062] In this regard, the follower vehicle can include wireless capabilities and a navigation computer with computer hardware integrated with its controller. This can allow the follower vehicle to be controlled through a wireless connection. In one embodiment, the interface can allow a vehicle’s role within the convoy, as a leader, followed followed etc. to be defined. Each unmanned follower vehicle can be triggered to start/stop mission wirelessly, which instructs the follower vehicle to start or stop following the preceding vehicle when it begins to move.

[0063] The system and method disclosed herein can provide a potential high/dual utility. In a military context, the system can be applied to combat service support (CSS) logistic operations where multiple vehicles might be required to drive in a convoy for resupply missions. Specifically, the system allows for unmanned convoys to operate and be operationalised as military autonomous vehicles. Alternatively, in a civilian nonmilitary context, the system can be applied to logistics trucks that often drive in a convoy to deliver large amounts of payload in the construction industry. The employment of the system disclosed herein would advantageously reduce the burden on drivers required to perform such operations and reduce the fuel consumption for logistics vehicles, improving both efficiency and safety.

[0064] The system enables follow-me functions between vehicles and an operator to aid in logistical operations using a convoy of vehicles, be it for military use or industrial non-military use. This is of particular relevance in view of the lack of GPS availability in mixed indoor-outdoor applications that pose huge challenges to existing follow-me technologies.

[0065] While the invention is described for the navigation and control of military or construction vehicles, it is understood that the invention is not so limited and can be used to assist with other types of motorized vehicles in logistical, transport or agricultural industries. It is also possible to use the invention with autonomous vehicles. One or more follower vehicles can use the invention to follow the path/trajectory of the lead vehicle.

■ Vehicle Hardware

[0066] Each vehicle can include a set of Ultra-Wide Band (UWB) radio nodes that perform vehicle measurements to obtain the range and bearing of a lead vehicle relative to a follower vehicle. In particular, the Ultra-Wide Band (UWB) radio nodes can perform two-way time-of-flight measurements in order to obtain the lead vehicle’s range, heading and bearing. The Ultra-Wide Band (UWB) radio nodes on the follower vehicle can be triggered to range with the Ultra-Wide Band (UWB) radio nodes on the lead vehicle to obtain relative measurements between the vehicles.

[0067] The UWB measurements (depicted in FIG. 1 as dashed and dotted lines) can provide highly precise object location services at the centimeter level and can also determine whether a vehicle is stationary, moving closer or moving away therefrom.

[0068] In one embodiment, the lead vehicle can comprise a first set of Ultra-Wide Band (UWB) radio nodes. The first set of UWB radio nodes can comprise at least two UWB beacons. In one embodiment, the first set of UWB radio nodes can comprise two UWB beacons.

[0069] The first set of Ultra-Wide Band (UWB) radio nodes can be positioned appropriately in order to achieve their function. In one embodiment, the first set of UWB radio nodes can be placed toward the rear of the vehicle, as wide apart as practicable and as high as possible from ground level to maximize 1 ) The line-of-sight to the follower vehicles nodes and 2) The base length between the sets of radio nodes. However, it will be appreciated that the orientation and positioning of the first set of UWB radio nodes can be varied without impacting the functioning of the system disclosed herein, as omnidirectional antennas can be used and are movable with software configuration.

[0070] In one embodiment, the first set of Ultra-Wide Band (UWB) radio nodes can be connected to a power supply for activation and an antenna. In one embodiment, the first set of UWB radio nodes can comprise two UWB beacons connected to a power supply for activation and an antenna.

[0071] In one embodiment, the follower vehicle can comprise a second set of Ultra- Wide Band (UWB) radio nodes. The second set of UWB radio nodes can comprise at least three UWB nodes. In one embodiment, the second set of UWB radio nodes can comprise three UWB nodes. The second set of Ultra-Wide Band (UWB) radio nodes can be positioned appropriately by the skilled artisan in order to achieve their function. In one embodiment, the second set of UWB radio nodes can be positioned to be laterally spaced as far apart as possible from one another on the follower vehicle and not be in a single straight line. In one embodiment, the second set of UWB radio nodes can be at the same elevation as their corresponding nodes on the lead vehicle. In this regard, the UWB nodes on the follower vehicle can be configured and positioned to obtain range measurements sequentially or non-sequentially with the UWB beacons on the lead vehicle. In one embodiment, the second set of UWB nodes comprises three nodes that can be positioned and mounted to maximize the lateral separation between the nodes, and ensure there is a longitudinal separation between at least one node and the other two nodes.

[0072] In one embodiment, the UWB radio nodes of both vehicles can be configured in a dynamic constellation. The dynamic constellation configuration can enable a ‘local’ pseudo-GPS system while the vehicles are moving, thus allowing relative localisation of the vehicles without reliance on a GPS reception. The dynamic constellation herein refers to the second set of UWB nodes on the follower vehicle comprising three or more UWB nodes, whereby once configured, the nodes are kept in a fixed position on the follower vehicle when in use. The position of the first and second set of UWB nodes can be changed and reconfigured via software.

[0073] In one embodiment, the UWB radio nodes on each vehicle are attached and fixed statically in an anchored position. In this regard, while the UWB radio nodes can be statically affixed onto each vehicle they are dynamic due to the movement of the vehicles. This contrasts with the traditional use of UWBs anchors which are normally positioned statically on a structure that is itself static.

[0074] In one embodiment, the first set of UWB nodes can be positioned toward the rear of the lead vehicle and the second set of UWB nodes can be positioned toward the front of the follower vehicle to maximize the visibility between leader and follower UWB nodes. The first and second set of UWB nodes can be positioned at the same elevation between all vehicles.

[0075] In one embodiment, the second set of Ultra-Wide Band (UWB) radio nodes can be connected to a navigation computer for activation and triggering ranging. In one embodiment, the second set of UWB nodes can be connected to a power supply, a data line to the navigation computer and an antenna.

[0076] In one embodiment, the lead vehicle does not contain a navigation computer or processor, whereas the follower vehicle contains a navigation computer operably connected with the second set of Ultra-Wide Band (UWB) radio nodes. However, the lead vehicle can include an Operator Control Unit (OCU) (e.g., a laptop computer) with an interface for transmission of INS data via radio.

[0077] The UWB radio nodes can operate at a high frequency range and in a section of the radio spectrum that is separate from congested bandwidths clustered at around 2.4 GHz. In one embodiment, the UWB radio nodes can operate in frequency range of 3.8GHz - 4.8GHz and be suited for this application due to its extremely accurate distance measurements, even at long ranges, low ranging latency and high level of resistance to multi-path and inter-pulse interference. As will be appreciated, additional UWB radio nodes can be applied when the convoy length of vehicle is increased or to enhance robustness between vehicles in said convoy. Specifically, the UWB radio nodes and their configuration can be scalable dependent on the number of vehicles and application, whereby additional UWB radio nodes on each vehicle can be implemented to increase the convoy length.

[0078] In one embodiment, each leader-follower vehicle pair can be operated with two UWB nodes on the preceding lead vehicle and three UWB nodes on the follower vehicle. For example, in a convoy of three vehicles as shown in FIG. 2, the lead vehicle 10 is shown as it moves forward while turning. A first follower vehicle 20 moves along the path created by the lead vehicle. A second follower vehicle 30 moves along the path created by the first follower vehicle. The dashed lines represent range measurements from the right beacon. Dotted lines represent range measurements from the right beacon.

[0079] As shown in FIG. 2, the first vehicle can include two UWB nodes, the second vehicle can include five nodes (i.e. , three UWB nodes connected to the lead vehicle and two UWB nodes to operate as a lead vehicle for the third vehicle) and the third vehicle can include three UWB nodes. Additional followers can be configured like the second vehicle and inserted between the first and third vehicle to increase the convoy length. All vehicles in the convoy can also adopt a hardware configuration like that of the second vehicle and be assigned to their roles via software (i.e., vehicle with 5 node configuration can be either a leader, a follower or the last follower in the convoy). It will be appreciated that each lead-follower vehicle pair of UWB nodes can be increased from 2 +3 to 4 + 6 to improve the robustness and accuracy of sensing if required due to operational needs or platform size. The use and inclusion of more nodes can improve the line-of-sight, reducing the frequency of lost ranges and hence improve accuracy of measurements and following of vehicles.

[0080] It is appreciated that UWB radio nodes can have a limited number of transmission channels which could limit the maximum number of active UWB radio nodes within a convoy. However, to overcome this possible limitation the UWB radio nodes can be used in a network operating mode that allows for distributed self-coordination of ranging measurements. The network can be applied to a sufficient number of UWB radio nodes for unmanned convoy applications.

[0081] Further, in consideration that UWB radios can have degraded accuracy at further distances which could implicate following distances and hence reduce maximum travelling speed, the coordinated acceleration/braking between vehicles could still allow high speed travelling with shorter following distances. The vehicle measure information available in the system can be utilised via modification to the software modules in allowing for coordinated acceleration/braking.

■ Vehicle Software

[0082] The use of Ultra-Wide Band (UWB) radio nodes and measurements obtained therefrom can be prone to errors without line-of-sight. Multiple nodes can be used to increase redundancy and reduce the chance of losing a line-of sight between the lead and follower vehicles. However, instead of employing multiple additional nodes, both the lead and follower vehicle can use an inertial navigation system (INS) to effectively fill in the gap of intermittent non-line-of-sight occurrences. The INS can estimate the bearing and trajectory of the lead vehicle for the follower vehicle to track. By coupling the UWB radio nodes and INS for each vehicle, the system can accurately estimate a trajectory of the leader’s actual path to enable safe following and avoid the dependence on absolute localisation sensors such as GPS. The INS in both vehicles can be connected via CAN Bus to the navigation computer and/or OCU. In addition, the INS can be connected to one or more GPS antennas for initialisation/calibration or to be used in GPS mode if available/preferred. The INS can primarily feed the navigation computer with its global position in Universal Transverse Mercator (UTM) and heading. The INS can also be connected to an odometry information source to improve the accuracy of its position measurements.

[0083] Accordingly, both the lead and follower vehicles can comprise an inertial navigation system to provide vehicle measures. The lead vehicle can comprise a first inertial navigation system and the follower vehicle can comprise a second inertial navigation system. The vehicle measure can include the longitudinal speed and the yaw rate of the vehicle, whereby these measurements are derived from the position information provided by the INS. [0084] The inertial navigation system can be a non-EUC (end-user computing) INS which can reduce the overall system cost without impinging on the operable function of the system. However, other navigation systems, such as a Vehicle Navigation System (VNS) for GPS-based leader-follower systems can be used in the system disclosed herein.

[0085] In order to stabilise readings from the INS an odometry information source can be connected to the INS in each vehicle to provide platform odometry information to the INS. In one embodiment, the odometry information source can be the vehicle CAN Bus, an odometer, such as an On-Board Diagnostic odometer (OBD ii), or wheel encoders. The odometry information source can be included in each vehicle to improve the endurance of the system and reduce jumps in vehicle pose which could affect stability of following performance.

[0086] In one embodiment, both the lead and follower vehicles can comprise an odometry information source. The odometry information source can be connected to the INS in both vehicles to provide said INS with odometry information. Thus, the lead vehicle can comprise a first odometry information source connected to the first inertial navigation system, and the follower vehicle can comprise a second odometry information source connected to the second inertial navigation system. The odometry information output from the odometry information source can be fed directly to the INS in each vehicle.

[0087] The navigation computer in the follower vehicle can be configured to execute one or more computer software configuration items (CSCI) for processing input data/information and vehicle measures for estimating the trajectory of the lead vehicle and producing a trajectory for the follower vehicle based on the lead and follower vehicle measures. In particular, the CSCI can include algorithm modules that utilise UWB radio node range values and vehicle measures, such as velocity and yaw rate, from both the lead and follower vehicles to produce a trajectory for the follower vehicle. The navigation computer can be a standard industrial processor. In one embodiment, the navigation computer can receive information and output from the first and second set of UWB nodes and INS to produce a trajectory for the follower vehicle to traverse. The desired trajectory can be passed through a path planner module, path follower module and vehicle control module which executes the drive-by-wire functions to control the follower vehicle mobility in order to follow the desired path of the lead vehicle.

[0088] In this regard, the set of UWB radio nodes on the lead vehicle (LV) and follower vehicle (FV) can communicate with each other to provide two-way time of flight data to obtain the LV’s range and bearing relative to the FV. Using background algorithms the Computer Software Configuration Item (CSCI), the range and bearings of the vehicles together with vehicle measurements (e.g. speed, yaw rate from the INS) can be used to estimate the heading/trajectory of the LV estimated.

[0089] Accordingly, a system is disclosed herein for coordinated movement of a plurality of vehicles, wherein a lead vehicle is followed by a follower vehicle to form a first lead-follower pair. The system can comprise a lead vehicle with a first set of Ultra-Wide Band (UWB) radio nodes, an OCU and a first inertial navigation system (INS) to provide lead vehicle measures; a follower vehicle with a second set of Ultra-Wide Band (UWB) radio nodes and a second inertial navigation system (INS) to provide follower vehicle measures; and a navigation computer configured to execute one or more computer software configuration items (CSCI) for estimating the trajectory of the lead vehicle and producing a trajectory for the follower vehicle based on the lead and follower vehicle measures.

[0090] An illustrative embodiment of the system disclosed herein is shown in FIG. 1. In this regard, the lead vehicle (10) is followed by a follower vehicle (20) to form a first lead-follower pair. The lead vehicle is shown as it moves forward while turning. The lead vehicle can comprise a first set of Ultra-Wide Band (UWB) radio nodes (12), more particularly two UWB beacons, an OCU (27), a first inertial navigation system (14) and a first odometry information source (16). Similarly, the follower vehicle (20) can comprise a second set of Ultra-Wide Band (UWB) radio nodes (22), more particularly three UWB nodes, a second inertial navigation system (24), a second odometry information source (26) and a navigation computer (28). In FIG. 1 , the ranging measurements between each node or beacon (12) of the first set of UWB radio nodes and each node (22) of the second set of UWB radio nodes is also depicted. The dashed lines represent range measurements from the right beacon of the lead vehicle. Dotted lines represent range measurements from the left beacon.

[0091] FIG. 2 shows an embodiment of the system disclosed herein when applied to a convoy of three vehicles. In this regard, the lead vehicle (10) is followed by a follower vehicle (20) to form a first lead-follower pair, with an additional third vehicle (30) forming a second lead-follower pair with the second vehicle (20). The lead vehicle can comprise a first set of Ultra-Wide Band (UWB) radio nodes, an OCU, a first inertial navigation system and a first odometry information source. The follower vehicle (20) can comprise two sets of Ultra-Wide Band (UWB) radio nodes (i.e. a second set of UWB radio nodes and a third set of UWB radio nodes), a second inertial navigation system, a second odometry information source and a navigation computer. The third vehicle (30) can comprise a fourth set of Ultra-Wide Band (UWB) radio nodes, a third inertial navigation system, a third odometry information source and a second navigation computer.

[0092] The navigation computer can include at least one processor configured to execute computer software configuration items stored on a computer readable storage medium. The storage medium can be a non-transitory computer-readable medium.

[0093] Accordingly, in one embodiment there is provided a computer readable storage medium, storing non-transitory instructions for controlling an automated processor to execute the method and computer software configuration items (CSCI) that can be implemented either on the system disclosed herein or another system configured to execute the instructions defining the computer software configuration items (CSCI) disclosed herein on said storage medium.

[0094] In one embodiment, at least one CSCI can be employed and executed by the navigation computer. In one embodiment, at least two CSCI can be employed and executed by the navigation computer. In one embodiment, at least three CSCI can be employed and executed by the navigation computer. In one embodiment, the CSCI can comprise an UWB CSCI, a Leader-Track Module (LTM) CSCI and a Pose Fusion CSCI.

[0095] FIG. 3 illustrates an embodiment of the interaction of CSCI’s that can be employed and executed by the navigation computer in obtaining an estimated trajectory of the lead vehicle. Specifically, it shows the interrelationship between modules of a follower vehicle (FV) and a leader vehicle (LV). Hardware components are depicted by solid boxes and software components are depicted by hollow boxes. The navigation PC components are highlighted with dashed lines and the OCU PC components are highlighted with dotted lines.

[0096] The flow of information and data obtained from the hardware components (UWB, INS, Odometry information source) of each vehicle is illustrated through the CSCIs in navigation computer or OCU PC. For a leader-follower pair, while one set of UWB radio nodes exists on both the leader and follower, the set on the leader vehicle does not interact with any CSCIs. The leader UWB radio nodes provide a beacon for the follower vehicle UWB radio nodes to range against, hence the UWB states are passed out of the UWB CSCI to the LTM CSCI in the navigation computer of the follower vehicle. To interface and obtain leader pose information from the leader vehicle INS, an instance of Pose Fusion CSCI exists on the Operator Control Unit computer in the Leader vehicle. The pose information, from both leader and follower vehicles, and UWB states from the follower vehicle are transmitted to the LTM CSCI for processing through a probabilistic filter.

[0097] The UWB CSCI can provide UWB states that are input into the LTM CSCI for further processing. The UWB CSCI can integrate and interface with UWB hardware that can include the UWB radio nodes of both lead and follower vehicles. In particular, the UWB CSCI can request for sequential or non-sequential range measurements from the UWB hardware to obtain UWB ranges of the vehicles. The UWB ranges can be input into a tracking filter to provide UWB states for tracking the dynamic relative range, heading and bearing of the lead vehicle relative to the follower vehicle. The tracking filter can be substituted with an appropriate linear quadratic estimation to provide the same function. In one embodiment, the tracking filter can be included in the UWB CSCI.

[0098] The UWB states herein refer to all the ranges between all follower UWB nodes and all leader UWB nodes, for example, where 2 UWB nodes exist on the leader vehicle and 3 UWB nodes exist on the follower vehicle, there will be 6 UWB states. The tracking filter can include a prediction step and an update step, where the filter uses the UWB states, calculates a prediction and associated error covariance of the preceding vehicle to be tracked, utilises actual UWB measurements to check the accuracy of its predictions, then adjusts gains to improve the accuracy of the following prediction step to effectively track the preceding vehicle’s range and bearing estimates. The tracking filter can be based off linear quadratic estimation and can include suitable transformations to handle system non-linearities.

[0099] The Pose Fusion CSCI can obtain pose readings from the INS on both lead and follower vehicles. Each vehicle runs its own instance of “Pose Fusion CSCI” to obtain pose readings from each respective INS. Accordingly, the Pose Fusion CSCI can be present in the leader vehicle to interface with the INS and transmit info to the LTM through a datalink. The Pose Fusion CSCI of the leader vehicle can exist on the Operator Control Unit (OCU), while the Pose Fusion CSCI of the follower vehicle can exist on the Navigation PC. Pose Fusion CSCI can output the UTM position (provided by the INS) and/or the vehicle measure (calculated by taking change in position over time). For the follower vehicle, both the UTM position and vehicle measures is input into the LTM. For the lead vehicle, only the vehicle measures is input to the LTM via the datalink for trajectory estimation.

[00100] Pose readings relate to the vehicle’s X,Y position and heading information. The pose readings can be obtained via a variety of sensor inputs (e.g., INS, GPS, perception SLAM, etc.). The pose readings obtained allow the system 1 ) to calculate vehicle measure (longitudinal velocity and yaw rate) by recording a change in pose over time; and 2) to function as a localisation and perception frame of reference for each follower vehicle requires a pose reading. The latter reason 2) can be global (true GPS UTM), pseudo global (pose given in UTM but might not be accurate/verified) or local (vehicle is given a starting position of 0,0 in its own frame of reference). In embodiments, the system is GPS denied and can work regardless of the true accuracy of the pose readings and only takes it as an arbitrary frame of reference.

[00101] In this regard, the output of the Pose Fusion CSCI can be input into the LTM CSCI. The LTM CSCI allows for future expansion to fuse other sources of pose readings. A datalink can be used to communicate the lead vehicle’s pose readings to the navigation computer for input into the Pose Fusion CSCI.

[00102] The Leader-Track Module (LTM) CSCI can provide an estimate of the lead vehicle trajectory. The LTM CSCI can comprise a series of steps in order to provide an estimation of the lead vehicles trajectory for the follower vehicle to follow. Initially, the LTM CSCI can obtain vehicle measures and UWB CSCI output (UWB states) to estimate the lead vehicle trajectory. The follower vehicle’s own pose readings can used to pin/place the lead vehicle’s trajectory within the follower vehicle’s own local frame of reference. In one embodiment, the LTM CSCI can comprise a probabilistic filter that receives the pose readings from both vehicles to provide kinematic leader and follower projections.

[00103] The UWB states output from the UWB CSCI can be input into the LTM CSCI to update the observation of the vehicle. Finally, based upon the initialization of the probabilistic filter, the LTM CSCI can provide an estimation of the lead vehicles trajectory as an output. The probabilistic filter can be a heuristic algorithm which utilises UWB states output from the UWB CSCI and vehicle measures from the follower and leader’s Pose Fusion CSCI in conjunction with pre-defined process noise values to produce an exact- path trajectory estimation of the preceding vehicle’s path. The probabilistic filter can obtain all inputs at each instance and utilizes the noise values to generate a large number of states which are then assigned weights based on measurement confidence in order to determine the most likely path of travel and hence the estimated trajectory.

[00104] In one embodiment, the probabilistic filter can process the estimated trajectory as a series of waypoints to traverse. Kinematic projections can be obtained by the probabilistic filter via the inputs and associated noise values. Further, observations can be obtained by the probabilistic filter from both the UWB nodes and vehicle measures. These observations can refer to the fused output of the leader position through fusion of the UWB information, Leader and Follower vehicle measures. These observations can be combined to produce the estimated leader trajectory.

[00105] In this regard, the estimated trajectory becomes the follower vehicle’s desired path of travel that can be passed through a path planning module that takes into account the perceived surroundings (obstacles etc.) and vehicle dynamics to plot a feasible, safe path for the follower vehicle to travel. The estimated trajectory and safe path can then be passed to a path follower module which generates a set of commands (speed, yaw rate) in order to reduce heading and cross track error of the follower vehicle. This set of commands can be passed to a vehicle control module that enacts direct control over the follower vehicle’s drive-by-wire system, enabling it to move and follow the path of the lead vehicle as closely as possible. The above-mentioned Path Planning Module (PPM CSCI), Path Following Module (PFM CSCI) and Vehicle Control Module (VCM CSCI) are additional modules that exist as separate software modules to the LTM CSCI. The PPM, PFM and VCM CSCI’s are downstream modules which utilize the trajectory produced by the LTM CSCI to perform the autonomous following. In one embodiment, the PPM, PFM and VCM CSCI’s can be generic modules which are required for most autonomous platform control and can differ based on the type of platform, robot etc.

[00106] In one embodiment, the lead vehicle can be a conventional, manually driven vehicle with no modifications made to the interface and/or controllers added for computer- based controls (i.e. , tele-ops or autonomous). Accordingly, the system and components disclosed herein can be retrofitted to vehicles as a kit.

[00107] In one embodiment, there is provided a kit for coordinated movement of a plurality of vehicles, wherein a lead vehicle is followed by a follower vehicle to form a first lead-follower pair, comprising: a first set of Ultra-Wide Band (UWB) radio nodes, an OCU, and a first inertial navigation system to provide lead vehicle measures; a second set of Ultra-Wide Band (UWB) radio nodes and a second inertial navigation system to provide follower vehicle measures; and a navigation computer configured to execute one or more computer software configuration items (CSCI) for estimating the trajectory of the lead vehicle and producing a trajectory for the follower vehicle based the lead and follower vehicle measures. As will be appreciated, the kit can include instructions for use and operation of the kit to said vehicles. The hardware of the follower vehicle is included under the assumption that the follower vehicle platform is drive-by-wire ready.

[00108] In one embodiment of the kit disclosed herein, a datalink, power source and odometry information source can be further included for each lead-follower pair. [00109] In one embodiment of the kit disclosed herein, the computer software configuration items (CSCI) can include Pose Fusion CSCI, UWB CSCI and LTM CSCI. Optionally, additional CSCI’s and modules can be included for path following, planning, Vehicle control, supervisory, communications and perception roles, such as PPM CSCI, PFM CSCI and/or VCM CSCI.

[00110] The kit disclosed herein can be independently fitted to a fully autonomous vehicle to allow unmanned followers to follow the fully autonomous vehicle. An unmanned follower vehicle can require either a native or retro-fitted drive-by-wire vehicle platform to enable computer-based controls. The unmanned follower vehicle can be navigated like an autonomous vehicle (i.e., it can technically plot a path and move from point to point).

[00111] A method of coordinated movement of a plurality of vehicles is disclosed herein, wherein a lead vehicle is followed by a follower vehicle. The method can employ the system disclosed herein to perform the steps outlined below and as illustrated in FIG.

4.

[00112] At step 101 , the method can be initiated by creating or organising a line of vehicles by positioning the follower vehicle behind the lead vehicle. The lead vehicle can be positioned in front of the follower vehicle within a +/- 45 degree cone of the follower vehicle’s forward direction. The relative heading between the lead vehicle and follower vehicle can be within +/- 45 degrees. Generally, the lead vehicle should be within a UWB range of the follower vehicle that can be approximately 50m.

[00113] To initiate the coordinated movement of the lead-follower vehicles, the unmanned follower vehicle can have an emergency stop mode inactivated, and can be toggled to an autonomous mode. The follower vehicle state can be in “idle”. The Operator Control Unit can be used to assign convoy roles to each vehicle (lead vehicle should know it is the lead vehicle etc.) before initiation. [00114] At step 102, once the vehicles are positioned appropriately in a line and assigned roles, the lead vehicle can be driven along a desired and intended route. In particular, upon triggering initiation, the lead vehicle can be driven along a desired and intended route and the unmanned follower vehicle can transit to an “auto idle” state and wait for waypoints to accumulate. Once there are feasible waypoints and sufficient distance between the lead vehicle and unmanned follower vehicle, the unmanned follower vehicle can begin to move and follow the path of the lead vehicle. The follower vehicle can begin to move once the distance/gap between both vehicles is sufficient. This distance/gap can be determined by the safe stopping distance of the follower vehicle at each commanded driving speed. The distance/gap between both vehicles can be maintained by the follower’s perception capabilities which ensures it performs adaptive cruise control to any obstacle in its path (including the leader which will always be at the end of the path).

[00115] At step 103, as the lead vehicle is driven the first and second set of UWB radio nodes obtain the lead vehicle range and bearing relative to the follower vehicle in addition to the vehicle measures of both vehicles.

[00116] At step 104, as the lead vehicle is driven, the navigation computer executes one or more computer software configuration items (CSCI) to obtain an estimate of the trajectory of the lead vehicle based upon the lead and follower vehicle measures and the lead vehicle range and bearing relative to the follower vehicle.

[00117] At step 105, based upon the estimate from step 104 a trajectory of the lead vehicle can be produced for the follower vehicle to be driven along in order to follow the exact path of the lead vehicle. As outlined above in relation to step 102, a distance/gap between both vehicles can be determined by the safe stopping distance of the follower vehicle at each commanded driving speed. The follower vehicle can have a fixed max desired velocity (e.g., 20km/h) that can be reduced or slowed based on the distance/gap between the vehicles or a distance to obstacle(s) in the path or end of path (i . e . , the leader vehicle itself can be such an obstacle).

[00118] During subsequent movement of the lead vehicle, the follower vehicle can repeat steps 103, 104, 105 to continuously track the leader vehicle.

[00119] In one embodiment, the method can comprise the steps of: creating a line of vehicles by positioning the follower vehicle behind the lead vehicle, wherein the lead vehicle comprises a first set of Ultra-Wide Band (UWB) radio nodes and a first inertial navigation system, and the follower vehicle comprises a second set of Ultra-Wide Band (UWB) radio nodes and a second inertial navigation system; driving the lead vehicle along an intended route; obtaining the lead vehicle range and bearing relative to the follower vehicle using the first and second set of UWB radio nodes; obtaining an estimate of the trajectory of the lead vehicle using a navigation computer configured to execute at one or more computer software configuration items (CSCI); and producing a trajectory for the follower vehicle to follow the lead vehicle.

[00120] In one embodiment, the step of obtaining the lead vehicle range and bearing relative to the follower vehicle includes obtaining sequential/non-sequential range measurements between the first and second set of UWB radio nodes. The sequential ranging measurements can be triggered by the navigation computer.

[00121] In one embodiment, the step of obtaining the lead vehicle range and bearing relative to the follower vehicle includes the first and second set of UWB radio nodes performing two-way time of flight measurements to obtain the lead vehicle’s range and bearing relative to the follower vehicle.

[00122] In one embodiment, the step of obtaining an estimate of the trajectory of the lead vehicle includes the navigation computer executing a UWB CSCI to provide UWB states, a Leader-Track Module (LTM) CSCI to obtain an estimate of the lead vehicle trajectory and a Pose Fusion CSCI to provide pose readings from both vehicles that are input into the LTM CSCI. The Pose Fusion CSCI of the leader vehicle can exist on the Operator Control Unit (OCU), while the Pose Fusion CSCI of the follower vehicle can exist on the Navigation PC.

[00123] The employment of the CSCI allow for the accurate functioning of the system in continuously obtaining the trajectory of the lead vehicle even during intermittent Non- Line-of-Sight (NLOS) conditions. This is in part due to the coupling of UWB radio nodes and INS and their respective vehicle measure output which provides clear advantages in comparison to perception-based Leader-Follower systems that rely heavily on maintaining a line-of-sight (LOS) between adjacent vehicles.

[00124] Further, perception-based Leader-Follower systems tend to degrade in performance in highly cluttered environments due to the presence of many surrounding edges/obstacles. Alternative radio frequency transmissions tend to be susceptible to multi-path effects which degrade ranging accuracy. In contrast, the UWB employed in the system disclosed herein is highly resilient to multi-path effects and is thus advantageous and functions accurately in cluttered environments.

[00125] The system and method disclosed herein, can be considered to include, but are not limited to one or more of the following advantageous features:

• Use of Ultra Wide Band (UWB) Radio;

• In an unmanned leader-follower setting;

• Nodes/Beacons need not be in a fixed/static anchor position, i.e. all can be mobile/dynamic;

• UWB radio complemented with an Inertial Navigation System (INS);

• Works without GPS;

• Utilises a tracking filter or linear quadratic estimation; • System can measure range, relative heading and bearing;

• Exact-path following instead of point-to-point following;

• System functions accurately during intermittent Non-Line-of-Sight (NLOS) conditions;

• System functions accurately even in cluttered environments;

• Scalable UWB node configuration and application;

• System is functional with cheap INS;

• Algorithm requires less computational resources than alternative perception-based Leader-Follower systems.

[00126] It will be appreciated that variations of the above disclosed and other features and functions, or alternatives thereof, may be combined into other systems or applications. Also, various unforeseen or unanticipated alternatives, modifications, variations or improvements therein may be subsequently made by those skilled in the art which are also intended to be encompassed by the following claims.

[00127] Although embodiments of the current disclosure have been described comprehensively, in considerable detail to cover the possible aspects, those skilled in the art would recognize that other versions of the disclosure are also possible.

Claims

33 CLAIMS What is claimed is:

1 . A system for coordinated movement of a plurality of vehicles, wherein a lead vehicle is followed by a follower vehicle to form a first lead-follower pair, said system comprising: a lead vehicle comprising a first set of Ultra-Wide Band (UWB) radio nodes and a first inertial navigation system to provide lead vehicle measures; a follower vehicle comprising a second set of Ultra-Wide Band (UWB) radio nodes and a second inertial navigation system to provide follower vehicle measures; and a navigation computer configured to execute one or more computer software configuration items (CSCI) for estimating the trajectory of the lead vehicle and determining a trajectory for the follower vehicle using the lead vehicle measures and follower vehicle measures.

2. The system of claim 1 , wherein the lead vehicle is manned and the follower vehicle is unmanned.

3. The system of claim 1 , wherein the lead vehicle and the follower vehicle are unmanned.

4. The system of claim 1 , wherein the lead vehicle further comprises a datalink and an Operator Control Unit (OCU) for user interface and communication of data.

5. The system of claim 1 , wherein the first set of UWB radio nodes comprises at least two UWB beacons.

6. The system of claim 5, wherein the second set of UWB radio nodes comprises at least three UWB nodes, wherein the at least three UWB nodes are 34 configured to obtain range measurements with at least two the UWB beacons.

7. The system of claim 6, wherein the first set of UWB radio nodes and the second set of UWB radio nodes are connected to the navigation computer to trigger ranging.

8. The system of claim 1 , wherein the first set of UWB radio nodes and the second set of UWB radio nodes perform two-way time of flight measurements to obtain the lead vehicle’s range and bearing relative to the follower vehicle.

9. The system of claim 1 , wherein the lead vehicle further comprises a first odometry information source connected to the first inertial navigation system, and the follower vehicle comprises a second odometry information source connected to the second inertial navigation system.

10. The system of claim 1 , wherein each of the one or more computer software configuration items (CSCI) comprise: a) an ultra-wide band (UWB) CSCI to provide ultra-wide band (UWB) states, b) a Leader-Track Module (LTM) CSCI to provide an estimate of the lead vehicle trajectory, and c) a Pose Fusion CSCI to provide pose readings that are input into the LTM CSCI.

11 . The system of claim 10, wherein the UWB CSCI comprises a tracking filter to track the dynamic relative range and bearing of the lead vehicle.

12. The system of claim 10, wherein the LTM CSCI comprises a probabilistic filter to estimate the lead vehicles trajectory based on the UWB states, the lead vehicle measures and the follower vehicle measures.

13. The system of claim 10, wherein the Pose Fusion CSCI obtains pose readings from one or more follower vehicles, and wherein a datalink is used to communicate pose readings of the lead vehicle to the navigation computer.

14. The system of claim 1 , wherein the system is operable under GPS-denied conditions.

15. The system of claim 1 , wherein the system is configured to operate so that the follower vehicle substantially follows a path of the lead vehicle.

16. A method of coordinated movement of a plurality of vehicles wherein a lead vehicle is followed by a follower vehicle, said method comprised of the steps of: a) creating a line of vehicles by positioning the follower vehicle behind the lead vehicle, b) driving the lead vehicle along an intended route; c) obtaining lead and follower vehicle measures and the lead vehicle range and bearing relative to the follower vehicle using a first set of Ultra-Wide Band (UWB) radio nodes on the lead vehicle and a second set of Ultra-Wide Band UWB radio nodes on the follower vehicle; d) obtaining an estimate of the trajectory of the lead vehicle using a navigation computer configured to execute one or more computer software configuration items (CSCI); and e) producing a trajectory for the follower vehicle to follow the lead vehicle.

17. The method of claim 16, wherein the first set of UWB radio nodes comprises at least two UWB beacons.

18. The method of claim 16, wherein the second set of UWB radio nodes comprises at least three UWB nodes, wherein the second set of UWB nodes obtain range measurements sequentially with the UWB beacons.

19. The method of claim 16, wherein the second set of UWB nodes are connected to the navigation computer for triggering ranging.

20. The method of claim 16, wherein the first set of UWB radio nodes and the second set of UWB radio nodes perform two-way time of flight measurements to obtain the lead vehicle’s range and bearing relative to the follower vehicle.

21 . The method of claim 16, wherein the lead vehicle further comprises a first odometry information source connected to a first inertial navigation system, and the follower vehicle comprises a second odometry information source connected to a second inertial navigation system.

22. The method of claim 16, wherein each of the at one or more CSCIs comprise: a) a UWB CSCI to provide UWB states, b) a Leader-Track Module (LTM) CSCI to obtain an estimate of the lead vehicle trajectory, and c) a Pose Fusion CSCI to provide pose readings that are input into the LTM CSCI.

23. The method of claim 22, wherein the UWB CSCI comprises a tracking filter to track dynamic relative range and bearing of the lead vehicle.

24. The method of claim 22, wherein the LTM CSCI comprises a probabilistic filter to estimate the lead vehicles trajectory based on the UWB states, lead vehicle measurements and follower vehicle measurements. 37

25. The method of claim 22, wherein the Pose Fusion CSCI obtains pose readings from one or more follower vehicles, and wherein a datalink is used to communicate the pose reading of the lead vehicle to the navigation computer.

26. The method of claim 16, wherein the lead vehicle is followed by the follower vehicle under GPS-denied conditions.

27. The method of claim 16, wherein the follower vehicle substantially follows a path of the lead vehicle.

28. A kit for coordinated movement of a plurality of vehicles, wherein a lead vehicle is followed by a follower vehicle to form a first lead-follower pair, the kit comprises: a first set of Ultra-Wide Band (UWB) radio nodes, an OCU, and a first inertial navigation system to provide lead vehicle measures; a second set of Ultra-Wide Band (UWB) radio nodes and a second inertial navigation system to provide follower vehicle measures; and a navigation computer configured to execute one or more computer software configuration items (CSCI) for estimating the trajectory of the lead vehicle and producing a trajectory for the follower vehicle based the lead and follower vehicle measures.