SE541604C2

SE541604C2 - Method and system for controlling a formation of vehicles during drive of said vehicles

Info

Publication number: SE541604C2
Application number: SE1750592A
Authority: SE
Inventors: Zhan Wang
Original assignee: Scania Cv Ab
Priority date: 2017-05-15
Filing date: 2017-05-15
Publication date: 2019-11-12
Also published as: SE1750592A1; DE102018003687A1; BR102018009046A2

Abstract

The present invention relates to a method for controlling a formation of vehicles (F) during drive of said vehicles. The method comprises the steps of: detecting (112, 114, 116) distances to at least an adjacent vehicle of said formation of vehicles (F) so as to create successive sets of virtual marks of said at least an adjacent vehicle; comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks; and controlling mutual positions (L1, D1 , L2, D2) of the vehicles (1, 2, 3) of said formation of vehicles (F) based on said comparisons so as to provide a determined formation of said formation of vehicles (F).The present invention also relates to a system for controlling a formation of vehicles during drive of said vehicles. The present invention also relates to a formation of vehicles, a computer program and a computer readable medium.

Description

METHOD AND SYSTEM FOR CONTROLLING A FORMATION OF VEHICLES DURING DRIVE OF SAID VEHICLES TECHNICAL FIELD The invention relates to a method for controlling a formation of vehicles during drive of said vehicles according to the preamble of claim 1. The invention also relates to a system for controlling a formation of vehicles during drive of said vehicles. The invention also relates to a formation of vehicles. The invention in addition relates to a computer program and a computer readable medium.

BACKGROUND ART In certain situations it is desired to drive vehicles in a certain formation. For example, when clearing snow from snow covered areas, a platoon of plough vehicles are normally used, the platoon of plough vehicles being arranged to plough in a certain formation in order to efficiently remove the snow. Such a formation of vehicles usually has a leading vehicle and trailing vehicles with a certain lateral displacement relative to the vehicle in front. In order to efficiently clear the snow the platoon the vehicles need drive in a determined formation.

During so called platooning vehicles are driving in a platoon on a road in a formation with a leading vehicle and trailing vehicles essentially aligned with the vehicle in front, said platoon of vehicles driving at certain distance relative to each other in order to reduce air resistance for the trailing vehicles so as to reduce fuel consumption.

US2010005688 discloses snow ploughing with plough vehicle driving in formations.

There is a need for improving controlling a formation of vehicles during drive of said vehicles.

OBJECTS OF THE INVENTION An object of the present invention is to provide a method for controlling a formation of vehicles during drive of said vehicles which is efficient and accurate.

Another object of the present invention is to provide a system for controlling a formation of vehicles during drive of said vehicles which is efficient and accurate.

SUMMARY OF THE INVENTION These and other objects, apparent from the following description, are achieved by a method, a system, a formation of vehicles, a computer program and a computer readable medium, as set out in the appended independent claims. Preferred embodiments of the method and the system are defined in appended dependent claims.

Specifically an object of the invention is achieved by a method for controlling a formation of vehicles during drive of said vehicles. Said formation of vehicles comprises at least two vehicles. The method comprises the steps of: detecting, by means of detection means on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle; comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks; and controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles.

By thus creating successive sets of virtual marks of said vehicle and comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks during drive of said formation of vehicles efficient and accurate determining of positions of said vehicles of said formation of vehicles relative to each other is facilitated. By then controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicle, such determined formation may be efficiently and accurately obtained and, depending on situation, maintained or adapted.

By thus detecting, by means of detection means, e.g. at least one laser scanner and/or at least one camera, on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles for creating said successive sets of virtual marks of said vehicle no need for expensive localization/positioning devices such as Real Time Kinematic (RTK) GPS are required.

By thus detecting, by means of detection means, e.g. at least one laser scanner and/or at least one camera, on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles for creating said successive sets of virtual marks of said vehicle, redundancy to other localization systems, if available, are obtained.

By thus detecting, by means of detection means, e.g. at least one laser scanner/LIDAR and/or at least one camera, on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles techniques for facilitating creating and comparing said successive sets of virtual marks of said vehicle such as LIDAR odometry, visual odometry and Kalman-filter based Simultaneous Localization And Mapping (SLAM), which techniques are very efficient and do not require much computation power.

One or more autonomous vehicles may advantageously be used in said formation of vehicles.

The step of detecting, by means of detection means on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle is performed intermittently.

Distances to at least an adjacent vehicle of said formation of vehicles are according to an embodiment detected at a certain frequency, e.g. a frequency in the range of 10-50 Hz. Distances to at least an adjacent vehicle of said formation of vehicles may be detected at any suitable frequency for facilitating determining mutual positions of vehicles of a formation of vehicles based on comparison of created virtual marks with successively created virtual marks.

Information, i.e. characteristics, of a created set of virtual marks with corresponding information, i.e. characteristics, of a subsequently created set of virtual marks during drive of the vehicle are compared intermittently.

The method thus comprises the step of creating successive sets of virtual marks based on information from detection means on board the vehicle intermittently detecting distances to at least an adjacent vehicle of said formation of vehicles. The step of creating successive sets of virtual marks based on information from detection means on board the vehicle intermittently detecting distances to at least an adjacent vehicle of said formation of vehicles is performed by means of virtual marks creation means. The virtual marks creation means may be any suitable means for processing signals/data representing detected range/bearings to at least an adjacent vehicle of said formation of vehicles. Said virtual marks creation means may according to an embodiment be comprised in said detection means. Said virtual marks creation means may according to an embodiment be an electronic control unit operably connected to said detection means.

According to an embodiment of the method the step of detecting distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks comprises detecting range and bearing relative to said at least an adjacent vehicle of said formation of vehicles.

Thus range and/or bearing relative to at least an adjacent vehicle of said formation of vehicles are detected by means of said detection means. Thus, the distance and angle to relative to at least an adjacent vehicle of said formation of vehicles are detected by means of said detection means. Said detection means for detecting distances/range/bearing to at least an adjacent vehicle of said formation of vehicles, e.g. one or more laser scanners/LIDAR’s and/or one or more cameras such as stereo cameras, are configured to intermittently detect at least an adjacent vehicle of said formation of vehicles during drive of the formation of vehicles by means of, at each detection moment, dispersing a large number signals/beams directed towards at least an adjacent vehicle of said formation of vehicles, i.e. determining the distance and angle of each signal/beams and creating a set of virtual marks for that particular detection moment. The set of virtual marks is according to an embodiment a two-dimensional set of virtual marks. The set of virtual marks is according to another embodiment a three-dimensional set of virtual marks. Said set of virtual marks may according to an embodiment be a so called point cloud, e.g. a two-dimensional point cloud or a three-dimensional point cloud.

Said large number of beams may be any suitable number of beams per detection moment for creating a set of virtual marks. Said large number of beams is according to an embodiment in the range of 100-20000 beams per detection moment for creating a set of virtual marks. Said large number of beams is according to an embodiment more than 500 beams per detection moment. Said large number of beams may have any suitable dispersion angle, e.g. 180 degrees, wherein said detection means, e.g. one or more detector on board a vehicle, is directed such that a set of said dispersed signals/beams will hit and thus detect the surface of at least an adjacent vehicle of said formation of vehicles for creating a set of virtual marks.

The step of comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks may be performed by means of any suitable comparison means comprising any suitable processor unit, control unit or the like. Said comparison means may comprise external comparison means arranged external to said vehicles of said formation of vehicles and operably connected to said vehicles of said formation of vehicles for receiving information of successively created virtual marks. Said comparison means may comprise internal comparison means arranged on board the respective vehicle of said formation of vehicles. Said comparison means may comprise means for performing scan matching.

The step of controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles is according to an embodiment performed automatically.

The step of controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles is performed by means of any suitable control means. Said control means comprises according to an embodiment electronic control units on board vehicles of said formation of vehicles.

The step of controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles is performed by means of any suitable control means comprises the step of controlling speed and/or steering of one or more of said vehicles of said formation of vehicles.

Said control means comprises according to an embodiment means for controlling the speed of one or more of said vehicles of said formation of vehicles. Said means for controlling the speed may be comprised in electronic control units arranged on board the respective vehicle of said formation of vehicles.

Said control means comprises according to an embodiment means for controlling steering of one or more of said vehicles of said formation of vehicles. Said means for controlling steering may be comprised in electronic control units arranged on board the respective vehicle of said formation of vehicles.

Information regarding detected distances, comparisons of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks, and/or control of mutual positions of the vehicles of said formation of vehicles based on said comparisons is according to an embodiment communicated between said vehicles of said formation of vehicles within a vehicle-to-vehicle communication arrangement.

Information regarding detected distances, comparisons of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks, and/or control of mutual positions of the vehicles of said formation of vehicles based on said comparisons is according to an embodiment communicated to said vehicles of said formation of vehicles within a vehicle-to-infrastructure communication arrangement.

According to an embodiment of the method the step of controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons comprises the step of controlling relative positions of each individual vehicle of said formation of vehicles based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles.

By thus controlling relative positions of each individual vehicle of said formation of vehicles based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles efficient control of the mutual positions of the vehicles for efficiently and accurately obtaining a determined formation of vehicles, e.g. efficiently and accurately maintaining a formation of vehicles.

Thus, if one or more a vehicles in said formation of vehicles deviates from a determined position relative to an adjacent vehicle a position adjustment of said one or more vehicles and/or other vehicles of the formation of vehicles may affect the determined formation of the formation of vehicles, wherein, according to this embodiment, care is taken to the relative positions of each individual vehicle when controlling the mutual positions of the vehicles based on said comparisons of information of information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks during drive of said formation of vehicles.

The formation of vehicles is thus treated as a single unit within which said control of mutual positions of vehicles are depending on the control and hence positon of each individual vehicle of said formation of vehicles.

As mentioned above, by detecting, by means of detection means, e.g. at least one laser scanner/LIDAR and/or at least one camera, on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles techniques for facilitating creating and comparing said successive sets of virtual marks of said vehicle such as LIDAR odometry, visual odometry and Kalman-filter based Simultaneous Localization And Mapping (SLAM), which techniques are very efficient and do not require much computation power. According to an embodiment the ego motion of each vehicle of said formation of vehicles are tracked by combining LIDAR scan matching based odometry technology and/or visual odometry technology, with vehicle nonholonomic motion constraint, i.e. due to the structure of the vehicle, at each time instance, the vehicle motion is rotating around the instantaneous centre of rotation (ICR).

By thus detecting, by means of detection means, e.g. at least one laser scanner/LIDAR and/or at least one camera, on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles for creating said successive sets of virtual marks of said vehicle, information of location of the current vehicle with respect to other vehicles in the formation for easily maintaining or adapting the formation of vehicles is obtained. The known distance between virtual marks on a neighbouring vehicle provides scale information to the visual odometry. A multiple vehicle SLAM algorithm based on extended Kalman filter (EKF) is according to an embodiment used to fuse the information from created successive sets of virtual marks to improve the estimate of the location of the whole formation of vehicles and the relative position of each individual vehicle within the formation of vehicles. This to ensure the whole formation of vehicles is proceeding as planned and, where applicable, the formation is well maintained.

According to an embodiment of the method created virtual marks of a vehicle comprises virtual marks of one or more of: left side of the vehicle, right side of the vehicle, rear side of the vehicle and front side of the vehicle.

According to an embodiment of the method said detection means on board a vehicle comprises at least one laser scanner and/or at least one camera.

Specifically an object of the invention is achieved by a system for controlling a formation of vehicles during drive of said vehicles. Said formation of vehicles comprises at least two vehicles. The system comprises detection means on board said at least one of said at least two vehicles arranged to detect distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle. The system further comprises means for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks; and means for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles.

According to an embodiment of the system the means for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons comprises means for controlling relative positions of each individual vehicle of said formation of vehicles based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles.

According to an embodiment of the system created virtual marks of a vehicle comprises virtual marks of one or more of: left side of the vehicle, right side of the vehicle, rear side of the vehicle and front side of the vehicle.

According to an embodiment of the system said detection means on board a vehicle comprises at least one laser scanner and/or at least one camera.

The system according to the invention has the advantages according to the corresponding method claims.

Specifically an object of the invention is achieved by a formation of vehicles comprising a system as set out herein.

Specifically an object of the invention is achieved by a computer program for controlling a formation of vehicles during drive of said vehicles, said computer program comprising program code which, when run on an electronic control unit or another computer connected to the electronic control unit, causes the electronic control unit to perform methods as set out herein.

Specifically an object of the invention is achieved by a computer readable medium comprising instructions which, when executed by a computer, cause the computer to carry out the method as set out herein.

BRIEF DESCRIPTION OF THE DRAWINGS For a better understanding of the present invention reference is made to the following detailed description when read in conjunction with the accompanying drawings, wherein like reference characters refer to like parts throughout the several views, and in which: Fig. 1 schematically illustrates a formation of vehicles performing a ploughing operation for clearing a snow covered area from snow; Fig. 2 schematically illustrates a block diagram of a system for controlling a formation of vehicles during drive of said vehicles according to an embodiment of the present invention; Fig. 3 schematically illustrates a block diagram of a method for controlling a formation of vehicles during drive of said vehicles according to an embodiment of the present invention; and Fig. 4 schematically illustrates a computer according to an embodiment of the present invention.

DETAILED DESCRIPTION Hereinafter the term “link” refers to a communication link which may be a physical connector, such as an optoelectronic communication wire, or a nonphysical connector such as a wireless connection, for example a radio or microwave link.

Hereinafter the term “formation of vehicles” refers to any formation kind of formation of any kind of vehicles. The term “formation of vehicles” may refer to a formation of plough vehicles driving in a formation suitable for ploughing, e.g. ploughing for clearing an area from snow, comprising a leading vehicle and trailing vehicles with a certain lateral displacement relative to the vehicle in front. The term “formation of vehicles” may refer to driving along a road performing so called platooning wherein the vehicles drive in a platoon with a leading vehicle and trailing vehicles essentially aligned with the vehicle in front. The term “formation of vehicles” may refer to a formation wherein vehicles drive next to each other, e.g. any kind of military formation. One or more of said vehicles of said formation of vehicles may be an autonomous vehicle.

Hereinafter the term “determined formation” refers to any formation during drive of said formation of vehicles comprising a predetermined formation determined prior to said drive and an adapted formation comprising a determined change of a current/originally determined formation of said formation of vehicles.

Fig. 1 schematically illustrates a formation of vehicles performing a ploughing operation for clearing a snow covered area from snow.

Said formation of vehicle F comprises three plough vehicles 1 , 2, 3 performing a ploughing operation for clearing a snow covered area A from snow, said vehicles 1 , 2, 3 driving in a certain vehicle formation F.

Said formation of vehicles F comprises a leading vehicle 1 and two trailing vehicles 2, 3. Said formation of vehicles F thus comprises a first plough vehicle 1 constituting the leading vehicle 1 , a second plough vehicle 2 constituting a trailing vehicle 2 to the first vehicle, and a third plough vehicle 3 constituting a trailing vehicle to the first plough vehicle 1 and second plough vehicle 2. The second plough vehicle 2 constitutes a leading vehicle to the third plough vehicle 3.

The first plough vehicle 1 has a plough 1a arranged in the front of the vehicle. The second plough vehicle 2 has a plough 2a arranged in the front of the vehicle. The third plough vehicle 3 has a plough 3a arranged in the front of the vehicle.

Said formation of vehicles F are driving in a determined formation. The determined formation comprises the second plough vehicle 2 driving at a distance D1 behind the first plough vehicle 1 and at a lateral position L1 relative to the first plough vehicle 1. The determined formation further comprises the third plough vehicle 3 driving at a distance D2 behind the second plough vehicle 2 and at a lateral position L2 relative to the second plough vehicle 2.

The formation of vehicles F performing a ploughing operation for clearing the snow covered area A from snow is controlled in accordance with the method and system as set out herein. According to an embodiment said formation of vehicles F comprises the system I or parts of the system I according to the present invention as described with reference to fig. 2 below. The system I comprises a control arrangement 100, which may comprise one or more electronic control units, said ploughing vehicles 1 , 2, 3 being operably connected to said control arrangement 100 via links.

The system I comprises a first detector 112 arranged on board the first vehicle 1. Said first detector 112 on board said first vehicle 1 is arranged to intermittently detect distances to the second vehicle 2 trailing the first vehicle 1.

The system comprises a second detector 114 arranged on board the second vehicle 2. Said second detector 114 on board said second vehicle 2 is arranged to intermittently detect distances to the first vehicle 1.

The system comprises a third detector 116 arranged on board the second vehicle 3. Said third detector 116 on board said third vehicle 3 is arranged to intermittently detect distances to the second vehicle 2.

Said first detector 112 may be a laser scanner or a camera, e.g. stereo camera, configured for range/bearing determination. Said second detector 114 may be a laser scanner or a camera, e.g. stereo camera, configured for range/bearing determination. Said third detector 116 may be a laser scanner or a camera, e.g. stereo camera, configured for range/bearing determination.

Said first detector 112 on board said first vehicle 1 is arranged to intermittently detect distances to the second vehicle 2 so as to create sets of virtual marks of said second vehicle 2. The first detector 112 detects the surface/parts of the surface of the second vehicle 2 so as to create successive sets of virtual marks of said second vehicle 2 by detecting range and bearing relative to said second vehicle 2.

Said second detector 114 on board said second vehicle 2 is arranged to intermittently detect distances to the second vehicle 2 so as to create sets of virtual marks of said first vehicle 1. The second detector 114 detects the surface/parts of the surface of the first vehicle 1 so as to create successive sets of virtual marks of said first vehicle 1 by detecting range and bearing relative to said first vehicle 1.

Said third detector 116 on board said third vehicle 3 is arranged to intermittently detect distances to the second vehicle 2 so as to create sets of virtual marks of said second vehicle 2. The third detector 116 detects the surface/parts of the surface of the second vehicle 2 so as to create successive sets of virtual marks of said second vehicle 2 by detecting range and bearing relative to said second vehicle 2.

Said first detector 112 for detecting range/bearing arranged on board said first vehicle 1 intermittently detects said second vehicle 2 by means of, at each detection moment, dispersing a large number beams/signals directed towards the second vehicle 2 which is hit by a set of beams B1 of said large number of beams, and determines range and bearing, i.e. distance and angle, of each beam/signal, for creating a set of virtual marks for that particular detection moment.

Said second detector 114 for detecting range/bearing arranged on board said second vehicle 2 intermittently detects said first vehicle 1 by means of, at each detection moment, dispersing a large number beams/signals directed towards the first vehicle 1 which is hit by a set of beams B2 of said large number of beams, and determines range and bearing, i.e. distance and angle, of each beam/signal, for creating a set of virtual marks for that particular detection moment.

Said third detector 116 for detecting range/bearing arranged on board said third vehicle 3 intermittently detects said second vehicle 2 by means of, at each detection moment, dispersing a large number beams/signals directed towards the second vehicle 2 which is hit by a set of beams B3 of said large number of beams, and determines range and bearing, i.e. distance and angle, of each beam/signal, for creating a set of virtual marks for that particular detection moment.

Information of created set of virtual marks is compared with corresponding information of a subsequently created set of virtual marks, this being performed intermittently, e.g. at a certain frequency, during drive of said formation of vehicles. Said comparison may be performed by means of said control arrangement 100 or other suitable comparison means. Said comparison may advantageously be performed utilizing techniques for facilitating comparing said successive sets of virtual marks of said vehicles 1 , 2, 3 comprising LIDAR odometry, visual odometry and Kalman-filter based Simultaneous Localization And Mapping (SLAM), which techniques are very efficient and do not require much computation power.

Mutual positions of the vehicles of said formation of vehicles F are controlled based on said comparisons so as to provide a determined formation of said formation of vehicles. Said control of mutual positions of said formation of vehicles F may be performed by means of said control arrangement 100 or other suitable control means. Said control is according to an embodiment performed automatically.

Said control of mutual positions of said formation of vehicles comprises controlling speed of said vehicles 1 , 2, 3 of said formation of vehicles F. Said control of mutual positions of said formation of vehicles comprises controlling steering of said vehicles 1 , 2, 3 of said formation of vehicles F.

Fig. 2 schematically illustrates a block diagram of a system I for controlling a formation of vehicles during drive of said vehicles according to an embodiment of the present invention. Said formation of vehicles comprises at least two vehicles.

The system I comprises a control arrangement 100. The control arrangement 100 may comprise one or more electronic control units. The control arrangement 100 may comprise one or more electronic control units in the respective vehicle of the formation of vehicles.

The system I comprises detection means 110 on board said at least one of said at least two vehicles arranged to detect distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle.

According to an embodiment of the system I created virtual marks of a vehicle comprises virtual marks of one or more of: left side of the vehicle, right side of the vehicle, rear side of the vehicle and front side of the vehicle.

According to an embodiment of the system I said detection means on board a vehicle comprises at least one laser scanner and/or at least one camera.

Said detection means 110 comprises one or more detectors 112, 114, 116 configured for range/bearing determination of vehicles of said formation of vehicles and being arranged on board one or more of said vehicles. Said detection means 110 comprises one or more detectors 112, 114, 116 configured for facilitating creating successive sets of virtual marks of at least adjacent vehicle/vehicles. Said one or more detectors 112, 114, 116 are in fig. 2 illustrated as a first detector 112 arranged on board a vehicle of said formation of vehicles to intermittently detect distances to at least an adjacent vehicle of said formation, a second detector 114 arranged on board a vehicle of said formation of vehicles to intermittently detect distances to at least an adjacent vehicle of said formation, and a third detector 116 arranged on board a vehicle of said formation of vehicles to intermittently detect distances to at least an adjacent vehicle of said formation. One vehicle of the formation of vehicles may have one or more detectors arranged on board the vehicle. Detectors need to be on board sufficient number of vehicles of said formation of vehicles in order to detect the positions of each vehicle relative to at least an adjacent vehicle of formation of vehicles.

According to an embodiment of the system I said detection means 110 on board said vehicle comprises at least one laser scanner/LIDAR and/or at least one camera, said detection means 110 being configured for determining distances from the vehicle on which said detection means is arranged to other vehicle/vehicles of the formation of vehicles. Thus, said one or more detectors 112, 114, 116 may be one or more laser scanners and/or one or more cameras.

Said at least one camera is according to an embodiment a stereo camera. Said at least one laser scanner and/or at least one camera may be arranged in any suitable way on a vehicle of the formation of vehicles, e.g. arranged on the chassis of a vehicle, and directed such that distances to at least an adjacent vehicle of said formation of vehicles may be detected.

Said detection means on board at least one of said at least two vehicles of said formation of vehicles may be already existing detection means on board said vehicle/vehicles which in addition to what they were intended to may be used for the system I for controlling a formation of vehicles during drive of said vehicles, said existing detection means being e.g. at least one laser scanner and/or at least one camera.

The detection means 110 on board said vehicle arranged to intermittently detect distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle is arranged to detect distances and angles to the surface of said vehicle/vehicles during drive of said formation of vehicles.

Said detection means 110 on board on board at least one of said at least two vehicles of said formation of vehicles is arranged to intermittently detect distances to the at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle. The detection means 110 detects the surface/parts of the surface of the said vehicle/vehicles as to create successive sets of virtual marks of said vehicle/vehicles by detecting range and bearing relative to said vehicle/vehicles.

Said detection means 110 for detecting range/bearing arranged on board at least one of said at least two vehicles of said formation of vehicles intermittently detects said vehicle/vehicles by means of, at each detection moment, dispersing a large number beams/signals directed towards said vehicle/vehicles which is/are hit by a set of beams of said large number of beams, and determines range and bearing, i.e. distance and angle, of each beam/signal, for creating a set of virtual marks for that particular detection moment.

According to an embodiment the system I comprises virtual mark creation means 120 for creating successive sets of virtual marks based on information from said one or more detectors 112, 114, 116 of said detection means 110 on board at least one of said at least two vehicles of said formation of vehicles intermittently detecting at least adjacent vehicle/vehicles. The virtual mark creation means 120 may be any suitable means for processing signals/data representing detected range/bearings to said vehicle/vehicles. Said virtual mark creation means 120 may according to an embodiment be comprised in said detection means 110. Said point cloud creation means may according to an embodiment be control arrangement 100 operably connected to said detection means 110.

The system I comprises means 130 for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks Said means 130 for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks may comprise any suitable processing means for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks during drive of the formation of vehicles comprising e.g. processing data representing such information/characteristics for such comparison. Said means 130 for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks during drive of the formation of vehicles is according to an embodiment comprised in said control arrangement 100.

Said means 130 for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks during drive of the formation of vehicles may comprise external comparison means arranged external to said vehicles of said formation of vehicles and operably connected to said vehicles of said formation of vehicles for receiving information of successively created virtual marks. Said external comparison means may comprise a server unit, processor, computer or the like. Said comparison means may comprise internal comparison means arranged on board the respective vehicle of said formation of vehicles. Said means 130 for comparing may comprise means for performing scan matching.

The system I comprises means 140 for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles.

According to an embodiment of the system I the means 140 for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons comprises means 140a for controlling relative positions of each individual vehicle of said formation of vehicles based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles.

Said means 140 for controlling mutual positions of the vehicles of said formation of vehicles comprises according to an embodiment means 142 for controlling the speed of one or more of said vehicles of said formation of vehicles. Said means 142 for controlling the speed may be comprised in electronic control units arranged on board the respective vehicle of said formation of vehicles.

Said means 140 for controlling mutual positions of the vehicles of said formation of vehicles comprises according to an embodiment means 144 for controlling steering of one or more of said vehicles of said formation of vehicles. Said means 144 for controlling steering may be comprised in electronic control units arranged on board the respective vehicle of said formation of vehicles.

Information regarding detected distances, comparisons of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks, and/or control of mutual positions of the vehicles of said formation of vehicles based on said comparisons is according to an embodiment arranged to be communicated between said vehicles of said formation of vehicles within a vehicle-to-vehicle communication arrangement. The system I comprises according to an embodiment a vehicle-to-vehicle communication arrangement, not shown.

Information regarding detected distances, comparisons of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks, and/or control of mutual positions of the vehicles of said formation of vehicles based on said comparisons is according to an embodiment arranged to be communicated to said vehicles of said formation of vehicles within a vehicle-to-infrastructure communication arrangement. The system I comprises according to an embodiment a vehicle-toinfrastructure communication arrangement, not shown.

By thus detecting, by means of said detection means 110, e.g. at least one laser scanner/LIDAR and/or at least one camera, on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles for creating said successive sets of virtual marks of said vehicle, information of location of the current vehicle with respect to other vehicles in the formation for easily maintaining or adapting the formation of vehicles is obtained. The known distance between virtual marks on a neighbouring vehicle provides scale information to the visual odometry. A multiple vehicle SLAM algorithm based on extended Kalman filter (EKF) is according to an embodiment used to fuse the information from created successive sets of virtual marks to improve the estimate of the location of the whole formation of vehicles and the relative position of each individual vehicle within the formation of vehicles. This to ensure the whole formation of vehicles is proceeding as planned and, where applicable, the formation is well maintained.

The control arrangement 100 is operably connected to the detection means 110 on board said at least one of said at least two vehicles arranged to detect distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle via a link 10. The control arrangement 100 is via the link 10 arranged to receive a signal from said detection means 110 representing data for detected distances to at least an adjacent vehicle of said formation of vehicles.

The control arrangement 100 is operably connected to said first detector 112 via a link 12. The control arrangement 100 is via the link 10 arranged to receive a signal from said first detector 112 representing data for detected distances to at least an adjacent vehicle of said formation of vehicles.

The control arrangement 100 is operably connected to said second detector 114 via a link 12. The control arrangement 100 is via the link 10 arranged to receive a signal from said second detector 114 representing data for detected distances to at least an adjacent vehicle of said formation of vehicles.

The control arrangement 100 is operably connected to said third detector 116 via a link 12. The control arrangement 100 is via the link 10 arranged to receive a signal from said third detector 116 representing data for detected distances to at least an adjacent vehicle of said formation of vehicles.

The control arrangement 100 is operably connected to the virtual mark creation means 120 for creating successive sets of virtual marks via a link 20a. The control arrangement 100 is via the link 20a arranged to send a signal to said means 120 representing data about detected distances to at least an adjacent vehicle of said formation of vehicles.

The control arrangement 100 is operably connected to the virtual mark creation means 120 for creating successive sets of virtual marks via a link 20b. The control arrangement 100 is via the link 20a arranged to receive a signal from said means 120 representing data about created sets of virtual marks.

The control arrangement 100 is operably connected to means 130 for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks via a link 30a. The control arrangement 100 is via the link 30a arranged to send signals to said means 130 representing data about information of successively created sets of virtual marks.

The control arrangement 100 is operably connected to the means 130 for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks via a link 30b. The control arrangement 100 is via the link 30b arranged to receive signals from said means 130 representing data about comparisons of information of subsequently created sets of virtual marks.

The control arrangement 100 is operably connected to means 140 for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles via a link 40. The control arrangement 100 is via the link 40 arranged to send signals to said means 140 representing data about controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles.

The control arrangement 100 is operably connected to means 140a for controlling relative positions of each individual vehicle of said formation of vehicles based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles via a link 40a. The control arrangement 100 is via the link 40a arranged to send signals to said means 140a representing data about controlling relative positions of each individual vehicle of said formation of vehicles based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles.

Fig. 3 schematically illustrates a block diagram of a method for controlling a formation of vehicles during drive of said vehicles according to an embodiment of the present invention. Said formation of vehicles comprises at least two vehicles.

According to the embodiment the method for controlling a formation of vehicles during drive of said vehicles comprises a step S1. In this step distances to at least an adjacent vehicle of said formation of vehicles are detected by means of detection means on board said at least one of said at least two vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle.

According to the embodiment the method for controlling a formation of vehicles during drive of said vehicles comprises a step S2. In this step information of a created set of virtual marks are compared with corresponding information of a subsequently created set of virtual marks.

According to the embodiment the method for controlling a formation of vehicles during drive of said vehicles comprises a step S3. In this step mutual positions of the vehicles of said formation of vehicles are controlled based on said comparisons so as to provide a determined formation of said formation of vehicles.

The step of detecting, by means of detection means on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said vehicle is performed intermittently.

The method thus comprises the step of creating successive sets of virtual marks based on information from detection means on board the vehicle intermittently detecting distances to at least an adjacent vehicle of said formation of vehicles.

Said detection means for detecting distances/range/bearing to at least an adjacent vehicle of said formation of vehicles, e.g. one or more laser scanners/LIDAR’s and/or one or more cameras such as stereo cameras, are configured to intermittently detect at least an adjacent vehicle of said formation of vehicles during drive of the formation of vehicles by means of, at each detection moment, dispersing a large number signals/beams directed towards at least an adjacent vehicle of said formation of vehicles, i.e. determining the distance and angle of each signal/beams and creating a set of virtual marks for that particular detection moment.

Said large number of beams may be any suitable number of beams per detection moment for creating a set of virtual marks with any suitable dispersion angle. Said large number of beams is according to an embodiment in the range of 100-20000 beams per detection moment for creating a set of virtual marks.

The step of comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks may be performed by means of any suitable comparison means comprising any suitable processor unit, control unit or the like.

By detecting, by means of detection means, e.g. at least one laser scanner/LIDAR and/or at least one camera, on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles techniques for facilitating creating and comparing said successive sets of virtual marks of said vehicle such as LIDAR odometry, visual odometry and Kalman-filter based Simultaneous Localization And Mapping (SLAM), which techniques are very efficient and do not require much computation power. According to an embodiment the ego motion of each vehicle of said formation of vehicles are tracked by combining LIDAR scan matching based odometry technology and/or visual odometry technology, with vehicle nonholonomic motion constraint.

With reference to figure 4, a diagram of an apparatus 500 is shown. The control unit 100 described with reference to fig. 2 may according to an embodiment comprise apparatus 500. Apparatus 500 comprises a non volatile memory 520, a data processing device 510 and a read/write memory 550. Non-volatile memory 520 has a first memory portion 530 wherein a computer program, such as an operating system, is stored for controlling the function of apparatus 500. Further, apparatus 500 comprises a bus controller, a serial communication port, l/O-means, an A/D-converter, a time date entry and transmission unit, an event counter and an interrupt controller (not shown). Non-volatile memory 520 also has a second memory portion 540.

A computer program P is provided comprising routines for controlling a formation of vehicles during drive of said vehicles. Said formation of vehicles comprises at least two vehicles. The program P comprises routines for detecting, by means of detection means on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle. The program P comprises routines for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks. The program P comprises routines for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles. The routines for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons comprises routines for controlling relative positions of each individual vehicle of said formation of vehicles based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles. The computer program P may be stored in an executable manner or in a compressed condition in a separate memory 560 and/or in read/write memory 550.

When it is stated that data processing device 510 performs a certain function it should be understood that data processing device 510 performs a certain part of the program which is stored in separate memory 560, or a certain part of the program which is stored in read/write memory 550.

Data processing device 510 may communicate with a data communications port 599 by means of a data bus 515. Non-volatile memory 520 is adapted for communication with data processing device 510 via a data bus 512. Separate memory 560 is adapted for communication with data processing device 510 via a data bus 511. Read/write memory 550 is adapted for communication with data processing device 510 via a data bus 514. To the data communications port 599 e.g. the links connected to the control units 100 may be connected.

When data is received on data port 599 it is temporarily stored in second memory portion 540. When the received input data has been temporarily stored, data processing device 510 is set up to perform execution of code in a manner described above. The signals received on data port 599 can be used by apparatus 500 for detecting, by means of detection means on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles so as to create successive sets of virtual marks of said at least an adjacent vehicle. The signals received on data port 599 can be used by apparatus 500 for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks. The signals received on data port 599 can be used by apparatus 500 for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles. The signals used for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons comprises signals used for controlling relative positions of each individual vehicle of said formation of vehicles based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles. The signals received on data port 599 can be used by apparatus 500 for controlling mutual positions of the vehicles of said formation of vehicles based on said comparisons so as to provide a determined formation of said formation of vehicles..

Parts of the methods described herein can be performed by apparatus 500 by means of data processing device 510 running the program stored in separate memory 560 or read/write memory 550. When apparatus 500 runs the program, parts of the methods described herein are executed.

The foregoing description of the preferred embodiments of the present invention has been provided for the purposes of illustration and description. It is not intended to be exhaustive or to limit the invention to the precise forms disclosed. Obviously, many modifications and variations will be apparent to practitioners skilled in the art. The embodiments were chosen and described in order to best explain the principles of the invention and its practical applications, thereby enabling others skilled in the art to understand the invention for various embodiments and with the various modifications as are suited to the particular use contemplated.

Claims

1. A method for controlling a formation of vehicles (F) during drive of said vehicles, said formation of vehicles comprising at least two vehicles (1, 2, 3), characterized by the steps of: - detecting (S1), by means of detection means (110) on board said at least one of said at least two vehicles, distances to at least an adjacent vehicle of said formation of vehicles (F) so as to create successive sets of virtual marks of said at least an adjacent vehicle; - comparing (S2) information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks; and - controlling (S3) mutual positions (L1, D1, L2, D2) of the vehicles (1, 2, 3) of said formation of vehicles (F) based on said comparisons so as to provide a determined formation of said formation of vehicles (F), wherein the step of controlling mutual positions of the vehicles (1, 2, 3) of said formation of vehicles based on said comparisons comprises the step of controlling relative positions of each individual vehicle (1, 2, 3) of said formation of vehicles (F) based on the control of the positions of the other vehicles of the formation of vehicles so as to provide said determined formation of said formation of vehicles (F).

2. A method according to claim 1, wherein created virtual marks of a vehicle comprises virtual marks of one or more of: left side of the vehicle, right side of the vehicle, rear side of the vehicle and front side of the vehicle.

3. A method according to claim 1 or 2, wherein said detection means (110) on board a vehicle comprises at least one laser scanner and/or at least one camera.

4. A system (I) for controlling a formation of vehicles (F) during drive of said vehicles, said formation of vehicles (F) comprising at least two vehicles (1, 2, 3), characterized by detection means (110) on board said at least one of said at least two vehicles arranged to detect distances to at least an adjacent vehicle of said formation of vehicles (F) so as to create successive sets of virtual marks of said at least an adjacent vehicle; means (130) for comparing information of a created set of virtual marks with corresponding information of a subsequently created set of virtual marks; and means (140) for controlling mutual positions (L1, D1, L2, D2) of the vehicles (1, 2, 3) of said formation of vehicles (F) based on said comparisons so as to provide a determined formation of said formation of vehicles (F), wherein the means (140) for controlling mutual positions of the vehicles of said formation of vehicles (F) based on said comparisons comprises means (140a) for controlling relative positions of each individual vehicle (1, 2, 3) of said formation of vehicles (F) based on the control of the positions of the other vehicles of the formation of vehicles (F) so as to provide said determined formation of said formation of vehicles (F).

5. A system according to claim 4, wherein created virtual marks of a vehicle comprises virtual marks of one or more of: left side of the vehicle, right side of the vehicle, rear side of the vehicle and front side of the vehicle.

6. A system according to claim 4 or 5, wherein said detection means on board a vehicle comprises at least one laser scanner and/or at least one camera.

7. A formation of vehicles (F) comprising a system (I) according to any of claims 4-6.

8. A computer program (P) for controlling a formation of vehicles during drive of said vehicles, said computer program (P) comprising program code which, when run on an electronic control unit (100) or another computer (500) connected to the electronic control unit (100), causes the electronic control unit to perform the steps according to claim 1-3.

9. A computer readable medium comprising instructions which, when executed by a computer, cause the computer to carry out the method according to anyone of claim 1-3.