The present invention relates to a method for preventing collisions, a computer program product for implementing said method and a collision avoidance system. More particularly, it relates to a method, computer program product and system for avoiding collisions between vehicles and vulnerable road users, including cyclists and pedestrians, and it provides significant benefits over current techniques in terms of its accuracy and effectiveness in detecting and preventing potential collisions.

Collision avoidance systems were developed by automobile manufacturers for vehicles in order to actively reduce the likelihood of potential collisions and the severity of road accidents. These systems typically use radar, lasers or cameras at the front and rear of the vehicle to detect an imminent crash and then they provide a warning or perform an autonomous aversion technique such as automatic braking or steering so as to avert the imminent crash. A typical collision avoidance system 10 is shown in Figure 1, and it comprises a potential hazard evaluation system 11, which takes inputs from the situation recognition system 12 (comprising forward and back CCD cameras, radar or lasers) and the vehicle sensors (comprising wheel speed sensors and acceleration sensors) and then outputs control signals to a warning system 13 (comprising a buzzer, head-up display and stop lamp) and the brake control system 15 of the vehicle (comprising a brake actuator and a sub-throttle accelerator) . Some known collision avoidance systems employ multi-stage collision avoidance schemes whereby depending on the likelihood or severity of collision predicted, different levels of collisions aversion techniques may be applied; when the likelihood of collision is categorised as low, the collision avoidance system may activate a warning light or another alarm signal within the vehicle in order to alert the driver, and when the likelihood of collision is categorised as high, the collision avoidance system may apply an automatic brake to the vehicle .

Vehicles having collision avoidance systems are often also equipped with adaptive cruise control, which use radar or lasers to automatically adjust the speed of a vehicle in order to maintain a safe distance from vehicles in front. Sometimes, collaborative adaptive cruise control systems are used so that information may be exchanged between a number of road users, resulting in a sequence of road users in a single lane that are all equidistant from one another, thus further reducing the likelihood of collisions. However, despite the attempts that have been made to reduce the number of road collisions, traffic accidents remain a major cause of injuries and fatalities. Whilst head-on collision prevention systems have occupied a predominant share of the market in the past, many collisions actually occur at busy intersections in urban areas, where there is usually a high intensity of activities and the visibility of the drivers may be reduced at the corners, yet current collision avoidance systems are not always well suited to detect and react to these types of potential collision.

The present inventors have recognised that there is a need for an improved collision avoidance system and method which address the problem of detecting collisions at intersections.

Thus, in accordance with one aspect of the present invention, there is provided a collision avoidance method for vehicles, the method comprising the steps of: detecting at one or more sensors mounted on a vehicle in use the position of an object relative to the vehicle at two or more time instances; receiving, at the input of a processor having at least one input and at least one output, the detected positions of the object from the one or more sensors; approximating the past trajectory of the object relative to the vehicle based on the detected positions; predicting the future position of the object relative to the vehicle using the approximated past trajectory; estimating the likelihood of collision of the vehicle with the object based on the predicted future position of the object relative to the vehicle; determining whether the likelihood of collision is over a predetermined threshold; and if the likelihood of collision is over the predetermined threshold, outputting an alarm signal from the output of the processor. With the present invention, a particular focus is placed upon side-to-side collisions between a vehicle making a turn (on the inside lane of the road in particular) and an object that is approaching along the side of the vehicle within the vehicle's turning radius (or at its nearside) . The position of the object relative to the vehicle is defined by the distance therebetween and its bearing information.

The sensor outputs, i.e. the detected distances, may be analogue or digital voltage outputs. The detected distances from at least two of the plurality of sensors may be filtered and then they are sent to and received by a processor, for example a real-time controller, for performing the further method steps in order to provide a collision avoidance system preferably in real-time. By detecting the relative position of the object at least at two time instances, the relative velocity of the object can be calculated. With detections at two time instances, it may be assumed that relative velocity of the object is constant. With three detections, it may be assumed that acceleration of the object relative to the vehicle is constant. With yet more detections, for example between ten and twenty detection events, the step of approximating the past trajectory of the object can be more accurate. It is preferred that the position of the object is detected at between ten and twenty time instances, preferably fifteen time instances, for a sensor reading rate of at least 7.5Hz .

The past trajectory of the object relative to the vehicle is approximated based on the detected positions, and the future position of the object relative to the vehicle is predicted using the approximated past trajectory. The provision of an active collision avoidance method (in contrast to a passive method) that takes into account the past trajectory of the object relative to the vehicle provides the advantage of more accurate prediction of the future position of the object. Once the future position of the object relative to the vehicle has been estimated, the processor determines whether the likelihood of collision of the vehicle with the object is over a predetermined threshold, and if the likelihood of collision is over the predetermined threshold, then an alarm signal is output from the output of the processor.

By using at least two detection events, it is possible to determine whether the object is moving or stationary. In the example that the object is a stationary object such as a lamppost or a post-box, the processor identifies the velocity of the object relative to the vehicle as a negative value (or the object may cease to be detected after for example the first detection event), which may result in the approximated past trajectory and the predicted future position of the object relative to the vehicle giving rise to the determined likelihood of collision being below the predetermined threshold. In the example that the object is a moving object, such as a pedestrian or another vehicle, which moves in a backwards relative direction to the vehicle, then the processor also identifies the velocity of the object relative to the vehicle as a negative value and thus the determined likelihood of collision may also be below the predetermined threshold. In the example that the object is a moving object, such as a pedestrian or another vehicle, which moves in a forwards relative direction to the vehicle, then the processor could identify the velocity of the object relative to the vehicle as being either negative or positive. In this example, it would be preferable to use more than two detection events in order to extract further information about the past trajectory of the object relative to the vehicle, for example, with three detection events, it is possible to identify the acceleration of the object.

The one or more sensors are preferably arranged substantially along a horizontal axis of a side of a vehicle in use. It may be preferable in some examples to use more than one sensor (for example, twelve sensors) so that a side of a vehicle may be substantially covered along the horizontal axis of a side of the vehicle in use.

The one or more sensors may be arranged along a horizontal axis arranged substantially along a top edge along a side of the vehicle in use, and each of said sensors may point downwardly towards the ground or towards the rear of the vehicle, or each of said sensors may be placed at a height of between 0.5 and 1.5 meters above the ground and point towards the rear of the vehicle. (This arrangement may be particularly advantageous if the one or more sensors comprise a position sensor such as a camera) . Alternatively, the one or more sensors may be arranged along a horizontal axis arranged preferably at a height of between 0.5 and 1.5 meters above the ground in use and each said sensors may point in the same direction, and more preferably, they may be arranged such that they are each orthogonal to and pointing outwardly from a side plane of the vehicle horizontally outwards from the vehicle. (This arrangement may be particularly advantageous if the one or more sensors comprise at least two distance sensors such as ultrasonic sensors) .

The one or more sensors may comprise at least two distance sensors, which may preferably be at least two ultrasonic sensors. In preferred embodiments, between ten and twenty ultrasonic sensors may be used, and they may be placed at approximately 1 meter intervals along a horizontal axis of the vehicle, and in a particular preferred example, twelve ultrasonic sensors may be used such that they are arranged substantially along and to substantially equidistantly cover a horizontal axis of a side of the vehicle. This way, they are particularly suited to detecting objects alongside the vehicle.

The one or more sensors may be calibrated and configured to detect the distances from each of said sensors to the object to be detected. These sensors are each capable of detecting distances from the vehicle to the object to be detected. By using at least two of said sensors, the bearing information may be further determined. More details on how this is implemented will be described below with reference to the Figures. In the example where at least two sensors are used and each individual sensor is capable of detecting the distance between the vehicle and the object to be detected at an instance in time, the sensors that are used may preferably be ultrasonic sensors. Ultrasonic sensors may be selected for use in examples of the claimed method for at least the following reasons: i. They can provide accurate and reliable measurements with a moving target;

ii. Output detections are possible at a rate of at least

7.5 detections per second;

iii. They can be suitable for outdoor use and in a variety of weather conditions;

iv. They may have internal temperature compensation;

v. The sensor outputs are easy to process with a real ^¬ time controller;

vi . They are compact and cost-effective; and

vii. They may provide a good range of about 0.3-5 meter and good measurement resolution of about 5mm.

The one or more sensors may alternatively or additionally comprise at least one position sensor, the position sensor being capable of determining distance and bearing angle from said sensor to the object, for example, a vision sensor (or video camera) (which may operate at infrared or visual wavelengths) , radar detector or LIDAR detector (or laser scanner) . The captured image (s) may then be processed by the processor in order to derive the position of the object in relation to the vehicle, including both distance from the vehicle and bearing information .

In a so called "fused" system where a combination of different types on sensors are used such that the outputs of the various sensors are integrated or fused to provide a more accurate detection of the position of the object relative to the vehicle at a particular time instance. In a fused system, the system may comprise one or more proximity sensors, which are binary detectors that can detect the presence of an object within a nominal range without any physical contact. These sensors may be used in conjunction with the one or more sensors in order to improve the accuracy of the system. In a fused system, the system may also comprise multiple proximity sensors and one or more cameras. Proximity sensors are for registering objects' distances to the vehicle while cameras facilitate identification of objects and assign proximity sensors' detections into different groups. The cameras can be used in conjunction with image processing software to distinguish between various different types of object adjacent to the vehicle (e.g. street furniture, parked cars, and several cyclists in a group) . This information may be ^x fused' with the data from the proximity sensors to give a more robust and reliable method for determining the motion of each cyclist. It is preferable that the method further comprises the step of filtering out irrelevant detections in order to derive a sequence of useful detections. This may be referred to as "ID filtering" . The step of approximating the past trajectory of the object may preferably be based on an optimisation technique. More preferably, the optimisation technique may minimise a cost function of the past trajectory of the object relative to the vehicle in order to select the most likely sequence of bearing angles associated with the one or more sensors at each time instance. The cost function is designed to ensure smooth trajectories and may use, for example, root mean square longitudinal acceleration or root mean square longitudinal velocity. The optimisation technique may preferably be quadratic programming. Quadratic programming provides a way of optimising a series of detected positions (distance and bearing angles) of the object over a number of time instances so as to produce a smooth trajectory. An assumption may be made in that longitudinal velocity and/or acceleration is constant.

The optimisation technique may additionally or alternatively comprise constructing inequality constraints by analysing the sequence of detections. This is especially useful in the example where at least two distance sensors, for example, two ultrasonic sensors, are used. When the one or more sensors comprise at least two distance sensors, the optimisation technique may preferably comprise setting equality constraints by triangulating the position of the object using distances measured at two adjacent sensors. The output of ultrasonic sensors is only distance information, and so use of a single ultrasonic sensor does not provide the bearing information needed to determine position relative to the sensor. To determine the bearing information, bearings are treated as unknowns in equations to solve. Triangulation between a pair of adjacent sensors may be used to provide additional bearing information, when the adjacent sensors have overlapping fields of view. This technique can be used to fix the position of the object relative to the vehicle at a time instance, and the bearing angles for the two sensors involved are derived which then serve as equality constraints for quadratic programming.

Triangulation between each pair of adjacent sensors may be performed. The triangulation method gives two bearings related to two sensors, and either one of the bearings may be used to express the estimated position of the object to be detected. The sensor located closer to the front of the vehicle is called the leading sensor and the other sensor in the triangle is called the trailing sensor. The step for predicting the trajectory of the object with respect to the vehicle may be based on both the detected distance from any one of the sensors and the bearing angle of said sensor, but preferably the detected distance and bearing angle of the sensor having a shorter detected distance to the object to be detected should be used out of the two sensors used to form the triangulation . For example, if the detected distance of the trailing sensor is greater than the detected distance of the leading sensor, then the detected distance and the bearing angle of the leading sensor may preferably be used to predict the future trajectory of the object. In another example, if the detected distance of the leading sensor is greater than the detected distance of the trailing sensor, then the detected distance and the bearing angle of the trailing sensor may preferably be used to predict the future trajectory of the object. In yet another example, if the detected distance of the trailing sensor is equal to the detected distance of the leading sensor, then the selection may be dependent upon previous detections.

The collision avoidance method may further comprise the step of smoothing the approximated past trajectory of the object using a Kalman filter. The trajectory of the object to be detected may be smoothed based on a kinematic model of the object's relative motion with respect to the vehicle. The step of predicting the future trajectory of the object may be performed under the assumption that the acceleration and/or the yaw rate of the object relative to the vehicle is constant.

The method of an embodiment of the present invention may be used with a variety of vehicles inter alia lorries, trucks, buses, coaches, trams and cars. It is believed that the method is particularly well suited to be used with heavy goods vehicles, especially non-articulated construction vehicles. In urban areas, these vehicles share the road with other potentially vulnerable road users such as cyclists and motorcyclists, which can pose significant safety concerns. Vulnerable road users, for example, those that share the road with heavy goods vehicles include inter alia cyclists, motorcyclists and pedestrians. It is believed that the method is particularly well suited to detect two-wheeled vehicles such as bicycles and motorcycles (along with the cyclist and motorcyclist) and pedestrians.

When the vehicle is a heavy goods vehicle and the object to be detected is a two-wheeled vehicle, it has been found that a majority of accidents occur in a side-to-side manner. One reason that side collisions with heavy goods vehicles, and especially non-articulated heavy goods vehicles, are particularly problematic is that these vehicles are often large and therefore introduce more blind spots for their drivers. Thus, if a cyclist positions himself or herself around the heavy goods vehicle, say for example at the traffic lights of an intersection, then there is a chance that the driver of the heavy goods vehicle is not aware of the cyclist at all. Furthermore, in order to make turning, a heavy goods vehicle often has to pull in the opposite direction (for example, pulling right to make a left turn) initially to ensure that its turning angle is sufficient. This could deceive the cyclist into thinking that the heavy goods vehicle is not making a turn towards them (and instead is continuing straight on or making a turn away from the cyclist) , which may encourage the cyclist to move forwards into the blind spot of the vehicle driver. Then, the turning front wheels/corner of the vehicle can knock the cyclist down and the rear wheels of the vehicle can crush the cyclist. It is therefore a particularly preferred implementation of the method of present invention that the vehicle may be a heavy goods vehicle and/or the object to be detected is a two-wheeled vehicle such as a bicycle. Preferably, the alarm signal that is output from the processor may comprise a visual and/or audio alarm signal and the method may further comprise the step of sending the alarm signal to a visual/audio alarm in order to activate said alarm if the likelihood of collision is over the predetermined threshold. Additionally or alternatively, the alarm signal may comprise a braking signal and the method may further comprise the step of sending the braking signal to a braking system of the vehicle in order to active the brakes of the vehicle if the likelihood of collision is over the predetermined threshold and/or the alarm signal may comprise a steering signal and the method may further comprise the step of sending the steering signal to a control system of the vehicle in order to change the steering angle of the vehicle if the likelihood of collision is over the predetermined threshold.

The visual/audio alarm, when activated by the alarm signal comprising a visual/audio alarm signal, acts to alert the driver of the vehicle to a potential collision. The braking system of the vehicle, when activated by the alarm signal comprising a braking signal, acts to automatically apply the brakes of the vehicle without any input from the driver, either immediately or progressively. The control system of the vehicle, when stimulated by the alarm signal comprising a steering signal, acts to automatically change the steering angle of the vehicle without any input from the driver. Preferably, the steering angle of the vehicle may be changed in a direction opposing the direction in which the sensors are pointing. The step of sending a visual and/or audio alarm signal, the step of sending a braking signal, and the step of sending a steering signal may be implemented simultaneously or sequentially, and in any order. This provides for a multi-stage collision avoidance method, which may depend on the likelihood of collision that is estimated. Preferably, the sending of a visual and/or audio alarm signal represents a first stage in the method when a first predetermined threshold of likelihood of collision is reached, the sending of a steering signal may represent a second stage in the method when a second predetermined threshold of likelihood of collision is reached, and the sending of a braking signal may represent a third stage in the method when a third predetermined threshold of likelihood of collision is reached. Alternatively, the steering angle and the braking signal may be applied substantially simultaneously as a second stage in the method after the visual/audio alarm signal is sent. An advantage of sending a visual/audio alarm signal as a first stage is that a driver of the vehicle may be given an opportunity to avert the collision himself or herself without any further assistance from the collision avoidance system. This would avoid the need for automatic braking or steering signals being sent. However, if a higher predetermined threshold of likelihood of collision is reached, then the method may further comprise sending braking and/or steering signals to the braking system and/or control systems of the vehicle.

In an embodiment of the method of the present invention comprising sending an alarm signal to a visual/audio alarm only, the collision avoidance system that implements said method may be communicatively isolated from the control system(s), for example the braking system or the steering system, of the vehicle. On the other hand, in an embodiment of the method of the present invention comprising sending a braking signal and/or a steering signal to a braking system and/or control system of the vehicle respectively (in addition to or alternatively to the alarm signal which is sent to a visual/audio alarm), the collision avoidance system that implements said method may be communicatively coupled to the braking system and/or control system of the vehicle. In accordance with another aspect of the present invention, there is provided a computer program product for a collision avoidance method, the computer program product comprising: a memory device comprising instructions which when executed by a processor causes the processor to perform any or all of the method steps as described above.

In accordance with yet another aspect of the present invention, there is provided a collision avoidance system for vehicles, the system comprising: one or more sensors mounted on a vehicle in use for detecting the position of an object relative to the vehicle at two or more time instances; and a processor having at least one input and at least one output, the processor being configured to: receive at its input the detected positions from the one or more sensors; approximate the past trajectory of the object relative to the vehicle based on the detected positions; predict the future position of the object relative to the vehicle using the approximated past trajectory; estimate the likelihood of collision of the vehicle with the object based on the predicted future position of the object relative to the vehicle; determine whether the likelihood of collision is over a predetermined threshold; and if the likelihood of collision is over the predetermined threshold, output an alarm signal from the output of the processor. It will be appreciated that each of the system features mentioned the above methods and the advantages of the method according to an aspect of the present invention as described above further apply of the system of this aspect .

Each of the one or more sensors of the collision avoidance system may be mounted along a horizontal axis of a side of the vehicle in use, which provides for particular advantages in detecting and preventing side-to-side collisions or nearside collisions as described above. In some preferred examples where the sensors are ultrasonic sensors, each of the one or more sensors are arranged in use between 0.5 and 1.5 meters above the ground and/or the system may comprise two or more sensors and the distance between two adjacent sensors is between 0.5 and 1 meter and more preferably about 0.8 meters. With such arrangements, it is believed that the collision avoidance system is particularly suited for use where the vehicle is a heavy goods vehicle and the object to be detected is a two-wheeled vehicle or a pedestrian.

The system may preferably further comprise a guide cone for at least one or each of the sensors. In one example, said guide cone may be a plastic cup. This acts to reduce detection of secondary reflections by narrowing the detection beam of a sensor. As a result, detection stability may be improved.

Preferably, the system may further comprise a visual and/or audio alarm signal, the system further comprises an alarm, and the processor may be further configured to send the alarm signal to the alarm in order to activate said alarm if the likelihood of collision is over the predetermined threshold. Additionally or alternatively, the alarm signal may comprise a braking signal and the processor may be further configured to send the alarm signal to a braking system of the vehicle in order to brake the vehicle if the likelihood of collision is over the predetermined threshold and/or the alarm signal may comprise a steering signal and the processor may be further configured to send the alarm signal to a control system of the vehicle in order to change the steering angle of the vehicle if the likelihood of collision is over the predetermined threshold.

Certain preferred embodiments of the present invention will now be described by way of example only and contrasted to comparative examples with reference to the accompanying drawings, in which:

Figure 1 shows a block diagram representing the components of typical prior art collision avoidance system; Figure 2 shows an aerial view of visibility of a driver of a vehicle in accordance with an embodiment of the present invention ;

Figure 3A shows an example of a vehicle in accordance with an embodiment of the present invention;

Figure 3B shows an aerial view of a likely trajectory of a vehicle in accordance with an embodiment of the present invention ;

Figure 4 shows a flowchart illustrating an exemplary collision avoidance method in accordance with an embodiment of the present invention ; Figure 5A shows the operation of an exemplary sensor in accordance with an embodiment of the present invention;

Figure 5B shows the operation of a plurality of exemplary sensors in accordance with an embodiment of the present invention;

Figure 6 shows a flowchart illustrating an exemplary method for predicting the trajectory of an object to be detected in accordance with an embodiment of the present invention; Figure 7 shows two exemplary sensors in accordance with an embodiment of the present invention to illustrate how triangulation techniques may be performed; Figure 8A shows an exemplary collision avoidance system in accordance with an embodiment of the present invention whereby triangulation techniques are not used; and Figure 8B shows an exemplary collision avoidance system in accordance with an embodiment of the present invention whereby triangulation techniques are used.

One reason that side collisions with heavy goods vehicles are particularly problematic is that these vehicles are often large and therefore introduce more blind spots for their drivers. Even with the provision of additional mirrors, there are often large areas that are obstructed to the driver and the mirrors themselves may represent a further obstruction and/or distraction to the driver. Figure 2 shows an aerial map of the light hand side areas, at ground level, that are visible to the driver: area 21 via the rear window; area 22 via the wide angle rear view mirror; area 23 via the side close proximity mirror; area 24 via the plane rear view mirror; area 25 via the nearside side window, area 27 via the windscreen; and area 28 via the front projection mirror. Areas 26, 29 are not visible to the driver either directly or indirectly. It will be appreciated that similar maps may be drawn up for other elevations, thus achieving a complete 3D map of the areas that are visible and non-visible to the driver of the heavy goods vehicle. Thus, if a cyclist positions himself or herself around the heavy goods vehicle, say for example at the traffic lights of an intersection, then there is a chance that the driver of the heavy goods vehicle is not aware of the cyclist at all.

Furthermore, in order to make turning, a heavy goods vehicle 30 (as shown in Figure 3A) often has to pull in the opposite direction (for example, pulling right to make a left turn) initially to ensure that its turning angle is sufficient, as shown in Figure 3B. This could deceive the cyclist into thinking that the heavy goods vehicle is not making a turn towards them (and instead is continuing straight on or making a turn away from the cyclist) , which may encourage the cyclist to move forwards into the blind spot of the vehicle driver. Then, the turning front wheels/corner of the vehicle can knock the cyclist down and the rear wheels of the vehicle can crush the cyclist.

The structure of the real time collision avoidance system in accordance with an embodiment of the present invention is shown in Figure 4. It provides a solution to the problem as described above with reference to Figure 3 in a simple and effective manner compared with prior art systems. The motion of the object to be detected (in this case a cyclist) and the vehicle (in this case a HGV) in a global coordinate systems are transformed into relative motion of the object to be detect with respect to the vehicle in the vehicle's coordinate system. Assumptions of constant acceleration and constant yaw rate may hold true equally in the vehicle's coordinate system as in the global coordinate system due to the nature of vector summation for acceleration and yaw rates.

Expressing the relative motion of the object to be detected with respect to the vehicle offers the following benefits: i. Accurate vehicle motion information, such as acceleration and position, is no longer required, which makes the system simpler and saves computation time in a real time system; and ii. Less hardware equipment is needed for the collision avoidance system, and so expensive vehicle motion measurement systems (GPS+IMU) may be removed from the system, thus making it more commercially appealing.

Thus, the real-time collision avoidance system 40 of Figure 4 performs the following tasks: i. Process the outputs from ultrasonic sensors to estimate the cyclist's position (S41);

ii. Estimate time to avoidance for potential collision (S42); iii. Determine the cyclist position relative to the HGV based on the detected distances and predict the cyclist's future trajectory (S43) ;

iv. Estimate the likelihood of a collision (S44); and

v. Deliver an alarm signal from the output of the processor if necessary, which may be a braking command to the braking system of the vehicle (S45) .

Figure 5 shows how a cyclist's position may be estimated using a system 50A, 50B for avoiding a collision in accordance with an embodiment of the present invention. In this example, a plurality of ultrasonic sensors are used, and the output from the ultrasonic sensor is the distance d between the object to be detected 53, 56 (i.e. the target, which in this case may be a cyclist) and the sensor. However, with distance information alone, it is not possible to pinpoint the exact position of the target 53, 56 because the bearing angle from the sensor to the target is unknown. Thus, the position of the cyclist could be anywhere on an arc with the same radius as shown in Figure 5A. This is termed as position ambiguity. In order to construct the cyclist's trajectory relative to the vehicle, it is necessary to recover the bearing information so as to remove any position ambiguity . A coordinate system may be set to define the positions of the ultrasonic sensors on the vehicle as well as to describe the cyclist's position, as shown in Figure 5B. The origin may be set at the mid-point on the front edge of the vehicle, with the x- axis pointing upwards in the longitudinal direction of the vehicle and the y-axis orthogonal to the x-axis and pointing towards the left.

A flowchart S60 is shown in Figure 6 to illustrate how the position of the cyclist may be estimated in real time. Each ultrasonic sensor outputs the detected distance together with its sensor ID at each time step. A fixed size of matrix containing sensor IDs and detected distances at consecutive time steps is constructed in S61. This matrix is called detection matrix. There can be multiple detections at each time step due to 1), multiple objects in the detection zone; 2), two neighbouring sensors can pick up the same object. It is necessary to process the detection matrix to separate sensor IDs into different groups, with each group corresponding to an object. This process is termed as ^X ID grouping and tracking' (S63) . In S63, sensor detections that are irrelevant to the motion of a cyclist are removed. Optional step S62 is introduced as a camera based method that can be optionally used to assist the process in S63. In S62, a certain set of computer vision techniques are applied to locate any cyclist in the view, and this information is sent to S63 to facilitate grouping of IDs from ultrasonic sensors. The result from S63 are sent to S64 which checks if any feasible triangulations are formed between neighbouring sensor detections in each detection group. An optimization algorithm using a quadratic programming (QP) approach is then used to determine the best set of detection angles Θ and the corresponding positions of the cyclist (S65) . The output from S64 is considered in S65 for posing equality constraints to help determine detections angles. Finally, a Kalman filter is used to smooth the trajectory of the cyclist based on a kinematic model of the cyclist's relative motion (S66) .

Some of the steps of flowchart S60 of Figure 6 will now be described in further detail. A reference line 57 may be defined to indicate the bearing angle starting from each sensor and pointing normal to and outwardly from the side of the vehicle, as show in Figure 5B. The distance line 52, 55 from the sensor to the target is also defined. The angle from the reference line 57 to the distance line is defined as the target bearing Θ, and clockwise rotation of Θ is defined as positive. The cyclist position P _cy , P _cx may therefore be expressed relative to the vehicle position P _sy , P _sx by the following equations:

Prv = Ρ _ν + d ^■ COS0 (1) P _cx = P _sx + d ^■ sinQ ( 2 )

In the above equations, only the parameter Θ is unknown. Therefore, the problem essentially becomes how to choose a value for Θ given the detected distance as well as sensor' s position on the vehicle are known.

It is not possible to solve Equations (1) and (2) independently. Given a series of detected distances (di , d _∑ , ds , ... , d _n ) for a short period of time (ti,t2,...,t _n ) , we need to find out the corresponding bearings {6 _lr θ ₂ ,...,θ _η ) so that the cyclist positions can be determined; i.e. the following equations must be solved:

Pcy,i = Psy.i + di ^■ COS0; ( 3 )

Pcx.i = Psx.i + di ^■ sinOi ( 4 ) where i=l , 2 , ...n .

Using simple first order and second order differentiation, the cyclist's velocity and the acceleration can be obtained, as shown by the following equations:

where j=2,3,...n; and Jc=3,4, ...,η. V and A represent velocity and acceleration respectively and t is the time stamp for each detection. Given n samples, n-2 lateral and longitudinal acceleration terms, are obtained from Equations (7) and (8) .

It is reasonable to assume that the cyclist moves at constant accelerations both laterally and longitudinally during the period from tj to t _∑ . In general a cyclist won't move back and forth relative to the truck during a short time span, unless there is a sudden stop which is unpredictable.

The longitudinal acceleration A _cx can be expressed in terms d, P _sx and Θ:

(tfc ^— tfc-i) ¹ dk-2 sin9k-2

cx,k

(tfc-i ^— tk-2) ¹ (tfe ^— tfc-i)^ (9)

sin Θ

+ k-l fe-l

(tfc-i ^— tk-2) ¹ (tfe ^— tfc-i) ² (t _k _ ₁ - t _k _ ₂ ) ^■ d _k ^■ sin9 _k

+

(tfc-i ^— tk-2) ¹ (tfe ^— tfc-i) ²

(tfe-i ^— ^-2) ¹ tfe ^— tfc-i

where k=3,4 ...,n

For each acceleration term -4 _ca . _fe , the only unknowns are 6 _k -2, 6 _k -

₁ , and 6 _k . As the detection range for each sensor is narrow, small angle approximation can be used to replace sind with Θ, thus the problem can be linearized.

Constant Velocity Assumption

Assuming constant longitudinal velocity for the cyclist, a mathematical expression can be obtained as:

A _C x,i — A _CXi 2— · · · — A _cxrL — 0 (10) It is equivalent to assume that:

A-cx,! ⁼ ^cx,2 = ··· = A _cxn = 0 (11)

As we are interested in finding the best set of θ± (i=l,...n) that result in a smooth trajectory history for the cyclist, it is reasonable to transfer Equation (11) into a quadratic programming (QP) problem.

n

J =∑A ² _cx , _k (12) k=3

QP aims to optimise a quadratic function of several variables subjects to linear constraints on these variables. The variables Qi Q2 ' ' ' θη-i and θ _η can be expressed in a column vector:

Θ = [θ ₁ ,θ ₂ ,...,θ _η ] ^τ (13)

The standard formulation of quadratic programming can be written in terms of Θ:

/(Θ) =^Q ^T QQ + L0 (14) where Q is an n by n matrix and is called quadratic matrix, and L is a n-variable row vector and called linear matrix. The following equations represent some restraints that should be fulfilled:

(15) (16)

Equality constraint (15) would provide strict constraints on some of the variables in © and thus enable a more accurate optimisation results. We could use triangulation to pose some linear inequality constraints on some of the elements in ©.

The equality constraints can be formed by including the triangulation results. A _eq and B _eq are required to be of constant size as well. In constructing matrices A _eq and B _eq , a principle worth highlighting is the number of inequality constraints should not be equal or greater than the number of variables in ©, otherwise it would result in an over-constrained objective and QP cannot converge to a solution.

For the inequality constraint (16), each element in © must be limited to its own upper and lower boundaries. These upper and lower limits come from the expected range of field of the sensor. Apart from the upper and lower limits for each element in ©, the sensor ID sequence during the same period of time can provide an estimate of the motion trend for the cyclist and thus set more limits on ©.

If the cyclist overtakes the vehicle, the bearing associated with each sensor changes from -ve to +ve in each sensor' s detection range, and vice versa when the truck is overtaking the cyclist. If the cyclist stays in one sensor's range for a short period of time, it may be necessary to refer to the motion trend of the cyclist relative to the truck deduced from previous steps.

If the cyclist stays in one sensor's range for a longer period of time and there is no prior knowledge about cyclist's motion trend, it is safe to assume that the cyclist is travelling at a similar speed to the truck. In this case, it's impossible to pose further constraints on the portion of the cyclist other than upper and lower bounds.

Constant Acceleration Assumption

To account for the situation when the longitudinal acceleration of the cyclist is not zero but a constant value during the period to inspect (PTI), the objective described in Equation (12) could be adapted to the following format:

where A _cx is the actual longitudinal acceleration of the cyclist during the PTI . The objective in Equation (17) indicates that the minimum of J can be achieved when A _cxk equals A _cxr which is an unknown parameter for at the start of the optimisation as we have no a priori knowledge of the cyclist motion. Treating A _cx as a coefficient and the Equation (17) could be rewritten as:

J = 2_O ) ² - 2 ^■ A _cx ^■ A _CXik ) + (n - 3)A ² _CX (18) fc=3 fc=3

The term (n— 3) A _CX in Equation (18) doesn't contain any bearings and therefore it won't affect the optimisation results and will be neglected. The term 2 ^■ A _cx ^■ (∑ _k=3 A _cxk ) doesn't contain any quadratic elements of the bearing and therefore it would only affect the linear matrix L as defined in Equation (14) . The quadratic matrix Q won't be affect by the inclusion of A _cx .

Θ could only be solved when the value of A _cx is known. A method is suggested to find the initial estimate of A _cx . A number of acceleration values ranging from 2m/s ² to 2m/s ² with a resolution of 0.1m/s ² are sampled. With each acceleration sample for A _cx , a candidate for Θ can be derived running QP. Each candidate for Θ is passed to the Equations (3) and (4) to get the cyclist positions for the PTI and the accelerations using Equations (5) to (8) . The standard deviation of these accelerations is calculated. After running through all the candidates for Θ , a series of standard deviations is obtained. Comparing these standard deviations, the smallest deviation is chosen and the corresponding Θ is then selected as the best estimation for cyclist bearings. The very sample for A _cx that produces Θ is believed to be the best estimation for the acceleration during the PTI.

In practice, there may be some inaccurate detections (ultrasonic pings are not always reflected from the same point of the cyclist) and signal dropouts, and so it is hard to obtain a smooth trajectory for the cyclist purely based on quadratic programming . A further smoothing method is needed to produce a smoother trajectory from which velocities and accelerations can be derived for future position prediction. A Kalman filter can smooth the results derived from quadratic programming. Kalman filters are widely used in guidance, navigation and control systems for vehicles, as well as motion tracking. Using a model of the system, a Kalman filter can smooth a series of measurements observed over time, which contains noise and other inaccuracies.

The cyclist's motion can be described purely in terms of its kinematics. The state vector describing the kinematics is called S _kf and it is constructed by four elements:

Given cyclist's position(P _cy , P _cx ) , velocity ^an d acceleration (A _cy ,A _cx ) in both lateral and longitudinal directions, its motion at step k+1 could be described by the following discrete equations based on information at step k, with dt being the time difference between steps:

1

Pcy,k+1 ⁼ Pcy,k + ^cy,k ' dt +— · A _{cy k} · dt (20)

1

Pcx,k+l ⁼ Pcx.k + Vcx,k ' dt +— ¹ A _{cx k} ^■ dt (21)

Vcy,k+1 ⁼ Vcy,k + ^cy,k ^' dt (23)

Vcx,k+l ⁼ Vcx.k + A _{cx k} ^■ dt (24)

Considering Equations (20) to (24), the state space equation for Kalman filter can be written as:

Ukf,k+1 + ^w kf,k (25)

The control input U _kf is the cyclist acceleration, which can be assumed to be zero. w _kf represents Gaussian white noise in the process. This noise is assumed to have zero mean and a covariance matrix defined as Go.

The measurement equation is expressed as: M _kf , _k+1 =ll ° Q ] - S _kf , _k + v _kfik (26) v _kf is the measurement noise, which is also of Gaussian white distribution. The Kalman filter works in two stages: time update and measurement update. Time update, also known as prediction, is for predicting values of the current state variables and error covariance estimates to obtain the a priori estimates for the next time step. The following two equations are called time update equations, with Z being the covariance matrix:

Skf,k+i— A _k f ^■ S _k f _>k (27) Zkf,k+i ⁼ A _k f ¹ Z _k f _>k+1 ^■ A _k f ^T + G ₀ (28)

The measurement update equations incorporate a new measurement into the a priori estimate to obtain an improved a posteriori estimate. Once new measurements are available, these estimates are updated based on a weighted average of the estimates and measurements, with more weighting being assigned to estimates with higher certainty. The ^λΑ ' indicates the derived value is an estimate, and the superscript ^λ -' means an a priori estimate.

In the measurement update stage, the Kalman gain K is updated using Equation (29) . With the updated Kalman gain, the state S is recalculated considering measurement M in Equation (30) . The covariance matrix P is further updated in the meantime, shown in Equation (31) .

-1

Kk+i— Z _k f, _k +i ^' C k> f ipkf ^' Z _k f, _k +1 C _k f ^T + Ro) (29)

$kf,k+l ⁼ $kf,k+l + Kk+i ^' _k f, _k +i kf ^' ½/,fc+i (30)

^J kf, _k +1 = k _f K _k +i ' Ckf)Z _k f _k+1 (31)

The variable I _kf in Equation (31) is a 4 by 4 identity matrix Detailed values in matrices Z _kf , G ₀ , and R ₀ are shown below:

Equations (27) to (31) are the core equations in a Kalman filter which can be run recursively to find the best estimate of the state vector.

With reference now to Figure 7, an example method step of detecting an object's position relative to the vehicle using triangulation based on detected distances is described. By placing ultrasonic sensors close to each other, i.e. at distances of 0.8m apart (Ak) , triangulation techniques can be used to fix the position of the cyclist when the beams of two adjacent sensors overlap. If two neighbouring sensors US _k and US _k +i have overlapping detection ranges and the cyclist falls into the overlap area, then cased on the two detected distance d _k and d _k+ i , the cosine rule may be applied in order to find the sensor bearings according to Equations (3) and (4) .

The sensor bearings 6 _k and 6 _k+1 are found as:

The above triangulation technique may only produce true results if certain physical criteria are met, and so a check may be performed between adjacent sensors according to the following equations : d _k + A _k > d _k+1 (34) |d _fc - d _fc+1 1< A _fc (35) Once a triangulation is found, it may be necessary to check if the calculated bearings are valid or not by comparing them with the maximum angle that forms the field of view of the sensors. Given the detected distance, the bearing must be bound by sensor's field of view. The optimum spacing of the ultrasonic sensors is chosen so that a common detection between three adjacent sensors was unlikely. Thus, if three neighbouring sensors do give a reasonable detection at the same time, it may be assumed two separate objects had been detected.

The triangulation method gives two bearings related to two sensors, and either one of the bearings could be used to express the estimated cyclist position. The sensor located closer to the front of the vehicle is called the leading sensor (i.e. US _k of Figure 7) and the other sensor in the triangle is called the trailing sensor (i.e. US _k+ i of Figure 7) . During a quadratic programming stage, only one detected distance and its corresponding bearing angle are needed at each time, but preferably the detected distance and bearing angle of the sensor having a shorter detected distance to the object to be detected should be used. For example, if p _k is greater than p _k+ i , then the trailing sensor US _k+ i is selected and if p _k+ i is greater than p _k , then the leading sensor US _k is selected. If p _k equals p _k+ i , then the selection is dependent upon previous detections.

Figures 8A and 8B illustrate the effects that triangulation may have on a collision avoidance method in accordance with an embodiment of the present invention where an object to be detected (in this case, a cyclist) passes a series of sensors (in this case, ultrasonic sensors) . In Figure 8A, there is no overlap between the sensors' detection geometry while in Figure 8B the sensors have overlap in their detection geometry. The cyclist passes the detection range of sensor US10, US9, US8 and US7 during a short period of time (tn-7 to tn) for example 1 second; during the same time span (Period to Inspect or PTI), sensors US1 and US2 generate effective detections at some time steps due to the noise or another object. The real-time controller needs to be capable of ignoring sensor US1 and US2 as their detections would distort the predicted trajectory for a cyclist.

With reference to Figure 8A, if there is no triangulation at any time step of the PTI, a general method to perform ID filtering may be used. To begin with, any ID that gives a valid detection at a time step could be selected, and used to form a string of IDs for the PTI. An example sample of ID string could be: [US10 US10 US2 US 9 US1 US7 US7 US7], where the numbers in the string are extracted to give a pure numeric array [10 10 2 9 1 6 7 7] . There would be other combinations of these ID from each line in the table as well; for example, [10 10 2 9 8 8 7 10] could be another combination. After all of the possible ID combinations are found, they can be stored in a matrix (an ID storage matrix) . Then, the difference between sensor IDs may be calculated and an optimum sensor ID string may be found.

With reference to Figure 8B, if valid triangulations are found during the PTI, these sensor IDs associated with triangulations are stored. The IDs associated with shorter detected distances in each triangulation pair are served as the ID boundaries that divide the full ID string in the PTI into several ID sections. In each ID section, the same rules for filtering IDs are applied, i.e. : i. ID jumps are not allowed;

ii. Non-monotonic ID sections are not allowed; and

iii. Responding to distance outliers is not allowed.

Applying the process listed above, the best ID sequence may be found, for example: [10 10 9 9 8 8 7 7] . This ID string, together with the associated detected distances, is passed to the next step for bearing angle optimisation in a process called quadratic programming.

It will be appreciated that although some of the examples of the present invention described above refer to preferred embodiments, the principles of these examples may be applied to all aspects and embodiments of the present invention, including those defined in any of the claims. Furthermore, features that are described in the context of separate embodiments may be provided in combination in a single embodiment, and conversely, features that are described in the context of a single embodiment may also be provided separately or in any suitable sub ^¬ combination .