camera-based articulation angle sensing for heavy goods...

20
August 17, 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs To appear in Vehicle System Dynamics Vol. 00, No. 00, Month 20XX, 1–20 Camera-based articulation angle sensing for Heavy Goods Vehicles Christopher de Saxe a,b* and David Cebon c a Council for Scientific and Industrial Research, PO Box 395, Pretoria 0001, South Africa b University of the Witwatersrand, School of Mechanical, Industrial and Aeronautical Engineering, South Africa c Cambridge University Engineering Department, Trumpington Street, Cambridge CB2 1PZ, UK (Received 00 Month 20XX; accepted 00 Month 20XX) Articulation angle sensing is an essential component of manoeuvrability and stability con- trol systems for articulated heavy goods vehicles, particularly long combination vehicles. Existing solutions to this sensing task are limited by reliance on trailer modifications or information or by measurement accuracy, or both, restricting commercial adoption. In this paper we present a purely tractor-based sensor concept comprising a rear-facing camera and the parallel tracking and mapping (PTAM) image processing algorithm. The system requires no prior knowledge of or modifications to the trailer, is compatible with planar and non-planar trailer shapes, and with multiply-articulated vehicle combinations. The system is validated in full-scale vehicle tests on both a tractor semi-trailer combination and a truck and full- trailer combination, demonstrating robust performance in a number of conditions, including trailers with non-planar geometry and with minimal visual features. Average RMS measure- ment errors of 1.19 , 1.03 and 1.53 were demonstrated for the semi-trailer and full-trailer (drawbar and semi-trailer) respectively. This compares favourably with the state-of-the-art in the published literature. A number of improvements are proposed for future development based on the observations in this research. Keywords: articulated vehicles, articulation angle, computer vision, sensors 1. Introduction The movement of domestic freight is fundamental to the functioning and growth of both developed and developing nations [1]. Domestic freight transport in most countries is largely dominated by road haulage, with the majority of this freight being moved on heavy goods vehicles (HGVs). Although HGVs are favoured for most domestic freight movement due to the speed and flexibility of service, it is an expensive and carbon- intensive mode of transport compared to other modes such as rail. The growth of freight transport demand on limited infrastructure, coupled with ambi- tious CO 2 emission reduction targets, has driven technology and policy development to investigate and implement measures to improve the efficiency of road freight transport. Of the technological and policy measures explored, the use of ‘Long Combination Vehicles’ (LCVs) has proven to be one of the most impactful, with relatively minor technological and infrastructural barriers. LCVs (also known as ‘road trains’ or ‘high capacity vehicles’), are truck combinations * Corresponding author. Email: [email protected] 1

Upload: others

Post on 03-Aug-2020

6 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

To appear in Vehicle System DynamicsVol 00 No 00 Month 20XX 1ndash20

Camera-based articulation angle sensing for Heavy Goods Vehicles

Christopher de Saxeablowast and David Cebonc

aCouncil for Scientific and Industrial Research PO Box 395 Pretoria 0001 South AfricabUniversity of the Witwatersrand School of Mechanical Industrial and Aeronautical

Engineering South AfricacCambridge University Engineering Department Trumpington Street Cambridge CB2 1PZ UK

(Received 00 Month 20XX accepted 00 Month 20XX)

Articulation angle sensing is an essential component of manoeuvrability and stability con-trol systems for articulated heavy goods vehicles particularly long combination vehiclesExisting solutions to this sensing task are limited by reliance on trailer modifications orinformation or by measurement accuracy or both restricting commercial adoption In thispaper we present a purely tractor-based sensor concept comprising a rear-facing camera andthe parallel tracking and mapping (PTAM) image processing algorithm The system requiresno prior knowledge of or modifications to the trailer is compatible with planar and non-planartrailer shapes and with multiply-articulated vehicle combinations The system is validatedin full-scale vehicle tests on both a tractor semi-trailer combination and a truck and full-trailer combination demonstrating robust performance in a number of conditions includingtrailers with non-planar geometry and with minimal visual features Average RMS measure-ment errors of 119 103 and 153 were demonstrated for the semi-trailer and full-trailer(drawbar and semi-trailer) respectively This compares favourably with the state-of-the-artin the published literature A number of improvements are proposed for future developmentbased on the observations in this research

Keywords articulated vehicles articulation angle computer vision sensors

1 Introduction

The movement of domestic freight is fundamental to the functioning and growth of bothdeveloped and developing nations [1] Domestic freight transport in most countries islargely dominated by road haulage with the majority of this freight being moved onheavy goods vehicles (HGVs) Although HGVs are favoured for most domestic freightmovement due to the speed and flexibility of service it is an expensive and carbon-intensive mode of transport compared to other modes such as rail

The growth of freight transport demand on limited infrastructure coupled with ambi-tious CO2 emission reduction targets has driven technology and policy development toinvestigate and implement measures to improve the efficiency of road freight transport Ofthe technological and policy measures explored the use of lsquoLong Combination Vehiclesrsquo(LCVs) has proven to be one of the most impactful with relatively minor technologicaland infrastructural barriers

LCVs (also known as lsquoroad trainsrsquo or lsquohigh capacity vehiclesrsquo) are truck combinations

lowastCorresponding author Email cdsaxecsircoza

1

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Rigid truck (b) Tractor semi-trailer

(c) Truck and drawbar trailer (d) Truck and full-trailer

(e) Nordic combination (f) B-double

(g) Tractor semi-trailer and drawbar trailer

Figure 1 Common HGV and HCV combinations (illustrations from [11])

with two or more trailers able to carry more freight per truck than traditional truck com-binations Successful implementations and pilot projects in (amongst others) AustraliaFinland Sweden Netherlands and South Africa have demonstrated significant potentialfor fuel use and carbon reduction averaging 12 in the relatively modest South Africanpilot [2] and with savings of up to 30 in Australia [3]

An illustration of seven common HGV combinations is shown in Figure 1 The lastthree would be commonly deemed to be LCVs and the last four possess two pointsof articulation (at either a pintle hitch turntable or fifth wheel) Although LCVs canmake use of existing infrastructure (though may be constrained to certain routes dueto their length) wider adoption is limited by reduced manoeuvrability and potentiallycompromised high-speed stability due to the number of articulation points Emergingtechnologies such as active trailer steering [4ndash6] autonomous reversing [7] combinedbraking and steering control [8] and anti-jackknife control [9] have been shown to im-prove performance in this regard but wide-spread uptake has been limited by practicalcommercialisation constraints

A particular challenge for the roll-out of such technologies is the requirement for addi-tional vehicle sensors and for LCVs particularly the requirement for articulation anglesensing Assuming conventional trailers it is necessary for articulation angle sensing sys-tems to be based on the tractor unit where control processing and actuation signalsoriginate Currently however these technologies rely on either trailer-based articulationangle sensors with non-standard or experimental communication links or estimationtechniques which require knowledge of the trailer states and other non-standard sensors

In this paper we present a camera-based system for articulation angle measurementusing the parallel tracking and mapping algorithm (PTAM) [10] The system is entirelytractor-based and improves upon the state-of-the-art against several versatility and per-formance metrics

2

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

11 Related work

Existing trailer-based articulation angle measurement systems include a kingpin-mountedsensor from Vehicle Systems Engineering BV Netherlands (the lsquoVSE sensorrsquo) [12] thelsquoOrbisensersquo magnetic sensor by AB electronic Ltd [13] a custom string potentiometer so-lution [14] and the custom articulation angle sensor incorporated by TRIDEC (Nether-lands) as part of their active trailer steering system [15]

In addition to being trailer-based the above systems are all lsquocontactrsquo type sensors(with the exception of the Orbisense sensor) This constrains the flexibility for use withvarious tractors and trailers and trailer types For tractor-based sensing a good casecan be made for non-contact sensing In the literature non-contact sensing solutionsfor this task have largely utilised state estimation methods (making use of other sensorinformation and models) or cameras

State observer methods have been demonstrated by Bouteldja et al [16] (using a stateobserver and an Extended Kalman Filter) Chu et al [17] and Ehlgen et al [18] Moderatemeasurement accuracies were obtained but only Ehlgen et alrsquos system was demonstratedin field tests achieving a maximum measurement error of 5

More recent work has focussed on the use of cameras arguably for their versatility andsignificant advances in the performance of computer vision algorithms In 2009 Schikoraet al [19] present a system for tractor semi-trailers comprising an upwards-facing cameraon the tractor unit viewing an encoder plate fitted to the underside of the semi-trailer(hence not strictly a tractor-based solution) Another system for rigid drawbar trailerswas also proposed using a rearward-facing infrared camera on the towing unit and aseries of infrared diodes fitted to the trailer (again not strictly tractor-based)

In 2013 Caup et al [20] presented a system for rigid drawbar trailers using a rear-facing camera The system requires the hitch location and drawbar length (a feature ofthe trailer) to be known and utilised a template-matching image processing method Thesystem was validated in field tests Also in 2013 Harris [21] presented another rear-facingcamera solution for tractor semi-trailer combinations Three image processing methodswere assessed including homography decomposition stereo vision (with the addition of asecond camera) and a template-matching method which showed improvements over thefirst two methods in a limited evaluation All three methods required the trailer front tobe planar (flat)

Fuchs et al [22] presented a similar rear-facing camera concept in 2014 which requiresmarkers in a known location to be fitted to the trailer together with knowledge ofvehicle geometry and camera location (and so not strictly trailer-based) With perfectknowledge of camera location a sub-1 accuracy was obtained in simulation but errorsrose significantly when uncertainty in camera location was incorporated In 2015 Fuchset al added a Kalman Filter to their system improving the accuracy [23]

In 2019 De Saxe and Cebon presented a further development on the rear-facing cam-era and template matching approach of Harris [24] incorporating an Unscented KalmanFilter to smooth the noisy template-matching measurements The system relied on someknowledge of the trailer geometry (making the solution not fully tractor-based) but sen-sitivity to this was shown to be small High accuracy was demonstrated in simulationswhile field tests on a tractor semi-trailer showed reasonable accuracy compared to thestate-of-the-art with RMS errors of 08ndash18 A large contributing factor to the experi-mental errors was relative pitch and roll motion between tractor and trailer which wasnot included in the simulations

As a benchmark for the current research results from the above literature were inves-tigated to extract the maximum measurement errors εmax demonstrated and maximum

3

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

articulation angles assessed (Γmax) RMS errors would arguably serve as a better bench-mark but are not easily available in all the studies It is clear from where simulationtests have been followed by full-scale testing that simulation results are a relatively poorindication of performance It is hence crucial to carry out full-scale testing

12 Research gap and objective

The rear-facing camera configuration coupled with a suitable computer vision algorithmis the clear candidate for further development given its versatility scope for being en-tirely tractor-based and the prevalence of powerful modern computer vision algorithmsThe described vision-based methods were shown to be limited to planar trailers withthe exception of [20] which was limited to drawbar-type trailers Further they were notdemonstrated to be compatible with two or more articulation pointsmdashan important con-sideration for LCVsmdashor not strictly tractor-based requiring known markers or knowledgeof trailer dimensions

For this work it was deemed fundamental to develop a system which was independentof trailer dimensions type or planarity such that the system could be compatible witha wide range of trailer types in use today including box trailers flat-bed trailers tankertrailer car-transporters etc without modification Given the prevalence of LCVs with twoor more articulation points it was also deemed important for the system to be adaptablefor the measurement of multiple articulation angles

In summary the objective of this research was to develop a new camera-based systemwhich

(1) is entirely tractor-based with no reliance on measurements from or geometric infor-mation about the trailer

(2) does not rely on the existence of known visual features on the trailer(3) is compatible with planar and non-planar trailer shapes(4) is compatible with two or more articulation points and(5) offers acceptable measurement accuracy for control applications in the order of 1

degree and(6) measures articulation angles up to 90 degrees

2 System overview

Based on previous work a rear-facing camera configuration was adopted The camerawas mounted to the tractor unit and faced the front of the trailer This is shown inFigure 2 The example of a single-articulation tractor semi-trailer vehicle combination isdepicted but the system will be shown later to be compatible with other combinationtypes

21 PTAM-based image processing

For the image processing algorithm a Simultaneous Localisation And Mapping orlsquoSLAMrsquo approach was adopted Primarily adopted in robotics applications SLAM per-tains to the problem of simultaneously determining the position and orientation of amoving agent and constructing a map of its three-dimensional environment This is sim-ilar to the problem of trailer motion sensing in this case the agent and environment areinterchangeable with the tractor and trailer respectively The objective is to determine

4

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 2 Rear-facing camera arrangement (shown for the case of a tractor semi-trailercombination)

the orientation of the trailer relative to the tractor and to build a map of the trailerto ensure compatibility with various planar and non-planar trailer shapes This inter-changeability is possible provided the trailer occupies the majority of the camera field ofview

Many solutions to the camera-based or visual SLAM problem (vSLAM) exist someof which are optimised for single cameras multiple cameras or other sensor inputs andinclude FastSLAM [25] FastSLAM 20 [26] Graph-based SLAM [27] Topological SLAM[28] LSD-SLAM [29] ORB-SLAM [30] and ORB-SLAM2 [31]

For this work the Parallel Tracking And Mapping (PTAM) algorithm by Klein andMurray [10] was adopted PTAM is a novel and highly efficient implementation of mono-camera vSLAM in which the localisation and mapping processing tasks are formulated inparallel The PTAM algorithm has been optimised for localised as opposed to explorationapplications meaning that the system expects to operate with a single scene and not toconstantly move into new scenes requiring recurring map extensions Being optimised asa localised system PTAM has superior pose estimation efficiency and accuracy versusexploratory systems with obvious benefits for the articulation angle sensing task Thename derives from the parallel nature in which the localisation (tracking) and mappingtasks are carried out on two separate CPU threads

The algorithm as implemented for the articulation sensing task is summarised in Fig-ure 3 [32] For an unseen trailer initially the algorithm requires two images of the trailerfront to create the initial three-dimensional trailer map (creating a point cloud frommatched features in the stereo image pair) These are called lsquokeyframesrsquo In practicethese can be obtained by briefly manoeuvring the trailer to two offset orientations Thesystem must then be zeroed which is achieved through a brief straight driving manoeu-vre

The stereo initialisation step is important as this defines the initial map upon whichall pose updates are based A small translation of the scene between the two initialkeyframes is required to complete the stereo mapping process The exact size of thistranslation is not prescribed and does not need to be known For typical hand-heldcamera applications Klein and Murray arbitrarily assumed the size of this translation inworld co-ordinates to be 10 cm in order to generate the initial map using the five-pointalgorithm [33] For the trailer sensing application the exact magnitude is not known andcan vary meaning the resultant Cartesian map is only accurate up to a scale factor (ierelative dimensions and translations are accurate but the precise scale is unknown) Aswe are only concerned with rotations (ie articulation angle) this is of no consequence

Ongoing orientation measurement can be achieved though matching features in thecurrent image with features in the existing trailer map (tracking) FAST features (Fea-

5

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 3 Flowchart of the PTAM-derived articulation angle measurement method [32]

tures from Accelerated Segment Test) [34] are used for this (as well as in the stereoinitialisation) and are tracked between frames using a decaying velocity motion model(similar to an alpha-beta filter [35]) and a simple pinhole camera model A perspec-tive projection pin-hole camera model is used for this purpose This is summarised inEquation 1 [36 37]

w = K[

R T]Xξuξv

ξ

=

kufx 0 u0

0 kvfy v0

0 0 1

[R T] XYZ1

(1)

XY Z are the location of a feature point in world coordinates u and v are the pixelcoordinates of the feature in the image plane R and T are the 3 times 3 rotation matrixand 3 times 1 translation vector of the relative camera motion K is the camera calibrationmatrix containing the lsquointrinsicrsquo camera parameters fx and fy are the focal lengths inthe horizontal and vertical directions u0 and v0 represent the location of the opticalaxis ku and kv are scaling parameters for rectangular pixels and ξ is the scaling factor(unknown in this case due to the scale ambiguity of using a single camera)

Lens distortion is accounted for using the simplified FOV-model [38] In this case pixelcoordinates are assumed to be distorted by a purely radial distortion which is describedusing a single distortion coefficient s as follows (where XC YC ZC are the location of thefeature point in camera-centred coordinates)

[uv

]=

[u0

v0

]+

[fx 00 fy

]rprime

r

[XcZc

YcZc

](2)

6

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

where

r =

radicX2

c + Y 2c

Z2c

rprime =1

sarctan

(2r tan

s

2

)(3)

In parallel to the tracking thread the mapping thread checks if new features (andhence potential new keyframes) are evident This would happen in practise if the trailerangle increased to a point where a new side of the trailer was seen for the first time (ifnot captured in the initial two keyframes) If a new keyframe is detected it is added tothe existing map and the map is updated If no new keyframes are detected the existingmap is refined through bundle adjustment [39]

22 Articulation angle extraction

The C++ source code for PTAM is open source [40] and was modified for application tothe articulation sensing task The algorithm provides two primary outputs a 3-D map offeature point locations and the camera pose in the form of a 3 times 1 translation vector Tand a 3times3 rotation matrix R (see Equation 1) During stereo initialisation the rotationmatrix is initialised relative to a dominant plane found in the first two keyframes and thetranslation vector represents the motion of the camera relative to its original location

Of interest here is the rotation matrix and particularly the yaw angle of the camerarelative to the observed scene (ie the trailer) In order to extract the articulation anglethe rotation matrix is decomposed into a combination of sequential rotations about eachaxis known as Euler angles roll (φ) yaw (ψ) and pitch (θ) This is summarised asfollows

R =

R11 R12 R13

R21 R22 R23

R31 R32 R33

= Rz(φ)Ry(ψ)Rx(θ) (4)

where

Rz(φ) =

cosφ minus sinφ 0sinφ cosφ 0

0 0 1

Ry(ψ) =

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

Rx(θ) =

1 0 00 cos θ minus sin θ0 sin θ cos θ

These Euler angles may be extracted from the rotation matrix using a combination of

the above definitions and logic to reject multiple solutions The method of [41] was usedfor this purpose These Euler angles are in the camera co-ordinate frame however and

7

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

must be translated into the trailer coordinate frame as followsφψθ

trailer

=

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

φψθ

camera

(5)

The articulation angle is then

Γ = ψtrailer (6)

23 Implementation

The modified PTAM algorithm was implemented in C++ on a 64-bit Linux laptopcomputer with a dual-core 24GHz Core i3 processor and 4GB of RAM Steady frameprocessing rates of 20 fps were achieved The built-in camera calibration tool availablewith PTAM was used to calibrate cameras

3 Simulations

31 Vehicle model

As an initial validation a simple 3D computer model of a tractor semi-trailer was createdin Autodesk Inventor with a single degree of freedom for articulation angle The modelis shown in Figure 2 A virtual camera was fixed behind the tractor cab as shown Thisfunctions as a simple lsquopin-holersquo camera with no distortion Photo-realistic visual texturewas added to the trailer based on an available test unit owned by the Cambridge VehicleDynamics Consortium (CVDC)

Representative articulation angle signals were generated using a three degree-of-freedom lsquobicycle modelrsquo of a tractor semi-trailer vehicle combination (More details canbe found in [24]) The model was subjected to a square wave steer input yielding anapproximately sinusoidal articulation angle signal The articulation angle signal was thenfed into the CAD model to generate sequences of image data from the virtual camerarepresentative of the articulation angle signals

32 Results

Two tests were carried out one reaching a maximum articulation angle of 50 andanother reaching 90 The 90 case served to test performance when the side of the trailercame into view forcing the need for new keyframes to be captured Results are shownin Figure 4 Γlim represents the angle beyond which the front of the trailer disappearsfrom view and only the trailer side is visible (around 70)

In both tests the errors increase approximately in line with increasing articulationangle potentially due to the increasing oblique camera view relative to where the stereoinitialisation took place The maximum errors do not align perfectly with maximumarticulation angles but rather seem to peak at around plusmn30 This may be a result ofvariations in the initialisation process The magnitude of errors may also be dependanton the keyframes used in initialisation No obvious degradation of accuracy is evidentbeyond Γlim Accuracy is good relative to the the published literature while the practicalshortcomings have been overcome

8

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 2: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Rigid truck (b) Tractor semi-trailer

(c) Truck and drawbar trailer (d) Truck and full-trailer

(e) Nordic combination (f) B-double

(g) Tractor semi-trailer and drawbar trailer

Figure 1 Common HGV and HCV combinations (illustrations from [11])

with two or more trailers able to carry more freight per truck than traditional truck com-binations Successful implementations and pilot projects in (amongst others) AustraliaFinland Sweden Netherlands and South Africa have demonstrated significant potentialfor fuel use and carbon reduction averaging 12 in the relatively modest South Africanpilot [2] and with savings of up to 30 in Australia [3]

An illustration of seven common HGV combinations is shown in Figure 1 The lastthree would be commonly deemed to be LCVs and the last four possess two pointsof articulation (at either a pintle hitch turntable or fifth wheel) Although LCVs canmake use of existing infrastructure (though may be constrained to certain routes dueto their length) wider adoption is limited by reduced manoeuvrability and potentiallycompromised high-speed stability due to the number of articulation points Emergingtechnologies such as active trailer steering [4ndash6] autonomous reversing [7] combinedbraking and steering control [8] and anti-jackknife control [9] have been shown to im-prove performance in this regard but wide-spread uptake has been limited by practicalcommercialisation constraints

A particular challenge for the roll-out of such technologies is the requirement for addi-tional vehicle sensors and for LCVs particularly the requirement for articulation anglesensing Assuming conventional trailers it is necessary for articulation angle sensing sys-tems to be based on the tractor unit where control processing and actuation signalsoriginate Currently however these technologies rely on either trailer-based articulationangle sensors with non-standard or experimental communication links or estimationtechniques which require knowledge of the trailer states and other non-standard sensors

In this paper we present a camera-based system for articulation angle measurementusing the parallel tracking and mapping algorithm (PTAM) [10] The system is entirelytractor-based and improves upon the state-of-the-art against several versatility and per-formance metrics

2

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

11 Related work

Existing trailer-based articulation angle measurement systems include a kingpin-mountedsensor from Vehicle Systems Engineering BV Netherlands (the lsquoVSE sensorrsquo) [12] thelsquoOrbisensersquo magnetic sensor by AB electronic Ltd [13] a custom string potentiometer so-lution [14] and the custom articulation angle sensor incorporated by TRIDEC (Nether-lands) as part of their active trailer steering system [15]

In addition to being trailer-based the above systems are all lsquocontactrsquo type sensors(with the exception of the Orbisense sensor) This constrains the flexibility for use withvarious tractors and trailers and trailer types For tractor-based sensing a good casecan be made for non-contact sensing In the literature non-contact sensing solutionsfor this task have largely utilised state estimation methods (making use of other sensorinformation and models) or cameras

State observer methods have been demonstrated by Bouteldja et al [16] (using a stateobserver and an Extended Kalman Filter) Chu et al [17] and Ehlgen et al [18] Moderatemeasurement accuracies were obtained but only Ehlgen et alrsquos system was demonstratedin field tests achieving a maximum measurement error of 5

More recent work has focussed on the use of cameras arguably for their versatility andsignificant advances in the performance of computer vision algorithms In 2009 Schikoraet al [19] present a system for tractor semi-trailers comprising an upwards-facing cameraon the tractor unit viewing an encoder plate fitted to the underside of the semi-trailer(hence not strictly a tractor-based solution) Another system for rigid drawbar trailerswas also proposed using a rearward-facing infrared camera on the towing unit and aseries of infrared diodes fitted to the trailer (again not strictly tractor-based)

In 2013 Caup et al [20] presented a system for rigid drawbar trailers using a rear-facing camera The system requires the hitch location and drawbar length (a feature ofthe trailer) to be known and utilised a template-matching image processing method Thesystem was validated in field tests Also in 2013 Harris [21] presented another rear-facingcamera solution for tractor semi-trailer combinations Three image processing methodswere assessed including homography decomposition stereo vision (with the addition of asecond camera) and a template-matching method which showed improvements over thefirst two methods in a limited evaluation All three methods required the trailer front tobe planar (flat)

Fuchs et al [22] presented a similar rear-facing camera concept in 2014 which requiresmarkers in a known location to be fitted to the trailer together with knowledge ofvehicle geometry and camera location (and so not strictly trailer-based) With perfectknowledge of camera location a sub-1 accuracy was obtained in simulation but errorsrose significantly when uncertainty in camera location was incorporated In 2015 Fuchset al added a Kalman Filter to their system improving the accuracy [23]

In 2019 De Saxe and Cebon presented a further development on the rear-facing cam-era and template matching approach of Harris [24] incorporating an Unscented KalmanFilter to smooth the noisy template-matching measurements The system relied on someknowledge of the trailer geometry (making the solution not fully tractor-based) but sen-sitivity to this was shown to be small High accuracy was demonstrated in simulationswhile field tests on a tractor semi-trailer showed reasonable accuracy compared to thestate-of-the-art with RMS errors of 08ndash18 A large contributing factor to the experi-mental errors was relative pitch and roll motion between tractor and trailer which wasnot included in the simulations

As a benchmark for the current research results from the above literature were inves-tigated to extract the maximum measurement errors εmax demonstrated and maximum

3

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

articulation angles assessed (Γmax) RMS errors would arguably serve as a better bench-mark but are not easily available in all the studies It is clear from where simulationtests have been followed by full-scale testing that simulation results are a relatively poorindication of performance It is hence crucial to carry out full-scale testing

12 Research gap and objective

The rear-facing camera configuration coupled with a suitable computer vision algorithmis the clear candidate for further development given its versatility scope for being en-tirely tractor-based and the prevalence of powerful modern computer vision algorithmsThe described vision-based methods were shown to be limited to planar trailers withthe exception of [20] which was limited to drawbar-type trailers Further they were notdemonstrated to be compatible with two or more articulation pointsmdashan important con-sideration for LCVsmdashor not strictly tractor-based requiring known markers or knowledgeof trailer dimensions

For this work it was deemed fundamental to develop a system which was independentof trailer dimensions type or planarity such that the system could be compatible witha wide range of trailer types in use today including box trailers flat-bed trailers tankertrailer car-transporters etc without modification Given the prevalence of LCVs with twoor more articulation points it was also deemed important for the system to be adaptablefor the measurement of multiple articulation angles

In summary the objective of this research was to develop a new camera-based systemwhich

(1) is entirely tractor-based with no reliance on measurements from or geometric infor-mation about the trailer

(2) does not rely on the existence of known visual features on the trailer(3) is compatible with planar and non-planar trailer shapes(4) is compatible with two or more articulation points and(5) offers acceptable measurement accuracy for control applications in the order of 1

degree and(6) measures articulation angles up to 90 degrees

2 System overview

Based on previous work a rear-facing camera configuration was adopted The camerawas mounted to the tractor unit and faced the front of the trailer This is shown inFigure 2 The example of a single-articulation tractor semi-trailer vehicle combination isdepicted but the system will be shown later to be compatible with other combinationtypes

21 PTAM-based image processing

For the image processing algorithm a Simultaneous Localisation And Mapping orlsquoSLAMrsquo approach was adopted Primarily adopted in robotics applications SLAM per-tains to the problem of simultaneously determining the position and orientation of amoving agent and constructing a map of its three-dimensional environment This is sim-ilar to the problem of trailer motion sensing in this case the agent and environment areinterchangeable with the tractor and trailer respectively The objective is to determine

4

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 2 Rear-facing camera arrangement (shown for the case of a tractor semi-trailercombination)

the orientation of the trailer relative to the tractor and to build a map of the trailerto ensure compatibility with various planar and non-planar trailer shapes This inter-changeability is possible provided the trailer occupies the majority of the camera field ofview

Many solutions to the camera-based or visual SLAM problem (vSLAM) exist someof which are optimised for single cameras multiple cameras or other sensor inputs andinclude FastSLAM [25] FastSLAM 20 [26] Graph-based SLAM [27] Topological SLAM[28] LSD-SLAM [29] ORB-SLAM [30] and ORB-SLAM2 [31]

For this work the Parallel Tracking And Mapping (PTAM) algorithm by Klein andMurray [10] was adopted PTAM is a novel and highly efficient implementation of mono-camera vSLAM in which the localisation and mapping processing tasks are formulated inparallel The PTAM algorithm has been optimised for localised as opposed to explorationapplications meaning that the system expects to operate with a single scene and not toconstantly move into new scenes requiring recurring map extensions Being optimised asa localised system PTAM has superior pose estimation efficiency and accuracy versusexploratory systems with obvious benefits for the articulation angle sensing task Thename derives from the parallel nature in which the localisation (tracking) and mappingtasks are carried out on two separate CPU threads

The algorithm as implemented for the articulation sensing task is summarised in Fig-ure 3 [32] For an unseen trailer initially the algorithm requires two images of the trailerfront to create the initial three-dimensional trailer map (creating a point cloud frommatched features in the stereo image pair) These are called lsquokeyframesrsquo In practicethese can be obtained by briefly manoeuvring the trailer to two offset orientations Thesystem must then be zeroed which is achieved through a brief straight driving manoeu-vre

The stereo initialisation step is important as this defines the initial map upon whichall pose updates are based A small translation of the scene between the two initialkeyframes is required to complete the stereo mapping process The exact size of thistranslation is not prescribed and does not need to be known For typical hand-heldcamera applications Klein and Murray arbitrarily assumed the size of this translation inworld co-ordinates to be 10 cm in order to generate the initial map using the five-pointalgorithm [33] For the trailer sensing application the exact magnitude is not known andcan vary meaning the resultant Cartesian map is only accurate up to a scale factor (ierelative dimensions and translations are accurate but the precise scale is unknown) Aswe are only concerned with rotations (ie articulation angle) this is of no consequence

Ongoing orientation measurement can be achieved though matching features in thecurrent image with features in the existing trailer map (tracking) FAST features (Fea-

5

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 3 Flowchart of the PTAM-derived articulation angle measurement method [32]

tures from Accelerated Segment Test) [34] are used for this (as well as in the stereoinitialisation) and are tracked between frames using a decaying velocity motion model(similar to an alpha-beta filter [35]) and a simple pinhole camera model A perspec-tive projection pin-hole camera model is used for this purpose This is summarised inEquation 1 [36 37]

w = K[

R T]Xξuξv

ξ

=

kufx 0 u0

0 kvfy v0

0 0 1

[R T] XYZ1

(1)

XY Z are the location of a feature point in world coordinates u and v are the pixelcoordinates of the feature in the image plane R and T are the 3 times 3 rotation matrixand 3 times 1 translation vector of the relative camera motion K is the camera calibrationmatrix containing the lsquointrinsicrsquo camera parameters fx and fy are the focal lengths inthe horizontal and vertical directions u0 and v0 represent the location of the opticalaxis ku and kv are scaling parameters for rectangular pixels and ξ is the scaling factor(unknown in this case due to the scale ambiguity of using a single camera)

Lens distortion is accounted for using the simplified FOV-model [38] In this case pixelcoordinates are assumed to be distorted by a purely radial distortion which is describedusing a single distortion coefficient s as follows (where XC YC ZC are the location of thefeature point in camera-centred coordinates)

[uv

]=

[u0

v0

]+

[fx 00 fy

]rprime

r

[XcZc

YcZc

](2)

6

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

where

r =

radicX2

c + Y 2c

Z2c

rprime =1

sarctan

(2r tan

s

2

)(3)

In parallel to the tracking thread the mapping thread checks if new features (andhence potential new keyframes) are evident This would happen in practise if the trailerangle increased to a point where a new side of the trailer was seen for the first time (ifnot captured in the initial two keyframes) If a new keyframe is detected it is added tothe existing map and the map is updated If no new keyframes are detected the existingmap is refined through bundle adjustment [39]

22 Articulation angle extraction

The C++ source code for PTAM is open source [40] and was modified for application tothe articulation sensing task The algorithm provides two primary outputs a 3-D map offeature point locations and the camera pose in the form of a 3 times 1 translation vector Tand a 3times3 rotation matrix R (see Equation 1) During stereo initialisation the rotationmatrix is initialised relative to a dominant plane found in the first two keyframes and thetranslation vector represents the motion of the camera relative to its original location

Of interest here is the rotation matrix and particularly the yaw angle of the camerarelative to the observed scene (ie the trailer) In order to extract the articulation anglethe rotation matrix is decomposed into a combination of sequential rotations about eachaxis known as Euler angles roll (φ) yaw (ψ) and pitch (θ) This is summarised asfollows

R =

R11 R12 R13

R21 R22 R23

R31 R32 R33

= Rz(φ)Ry(ψ)Rx(θ) (4)

where

Rz(φ) =

cosφ minus sinφ 0sinφ cosφ 0

0 0 1

Ry(ψ) =

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

Rx(θ) =

1 0 00 cos θ minus sin θ0 sin θ cos θ

These Euler angles may be extracted from the rotation matrix using a combination of

the above definitions and logic to reject multiple solutions The method of [41] was usedfor this purpose These Euler angles are in the camera co-ordinate frame however and

7

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

must be translated into the trailer coordinate frame as followsφψθ

trailer

=

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

φψθ

camera

(5)

The articulation angle is then

Γ = ψtrailer (6)

23 Implementation

The modified PTAM algorithm was implemented in C++ on a 64-bit Linux laptopcomputer with a dual-core 24GHz Core i3 processor and 4GB of RAM Steady frameprocessing rates of 20 fps were achieved The built-in camera calibration tool availablewith PTAM was used to calibrate cameras

3 Simulations

31 Vehicle model

As an initial validation a simple 3D computer model of a tractor semi-trailer was createdin Autodesk Inventor with a single degree of freedom for articulation angle The modelis shown in Figure 2 A virtual camera was fixed behind the tractor cab as shown Thisfunctions as a simple lsquopin-holersquo camera with no distortion Photo-realistic visual texturewas added to the trailer based on an available test unit owned by the Cambridge VehicleDynamics Consortium (CVDC)

Representative articulation angle signals were generated using a three degree-of-freedom lsquobicycle modelrsquo of a tractor semi-trailer vehicle combination (More details canbe found in [24]) The model was subjected to a square wave steer input yielding anapproximately sinusoidal articulation angle signal The articulation angle signal was thenfed into the CAD model to generate sequences of image data from the virtual camerarepresentative of the articulation angle signals

32 Results

Two tests were carried out one reaching a maximum articulation angle of 50 andanother reaching 90 The 90 case served to test performance when the side of the trailercame into view forcing the need for new keyframes to be captured Results are shownin Figure 4 Γlim represents the angle beyond which the front of the trailer disappearsfrom view and only the trailer side is visible (around 70)

In both tests the errors increase approximately in line with increasing articulationangle potentially due to the increasing oblique camera view relative to where the stereoinitialisation took place The maximum errors do not align perfectly with maximumarticulation angles but rather seem to peak at around plusmn30 This may be a result ofvariations in the initialisation process The magnitude of errors may also be dependanton the keyframes used in initialisation No obvious degradation of accuracy is evidentbeyond Γlim Accuracy is good relative to the the published literature while the practicalshortcomings have been overcome

8

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 3: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

11 Related work

Existing trailer-based articulation angle measurement systems include a kingpin-mountedsensor from Vehicle Systems Engineering BV Netherlands (the lsquoVSE sensorrsquo) [12] thelsquoOrbisensersquo magnetic sensor by AB electronic Ltd [13] a custom string potentiometer so-lution [14] and the custom articulation angle sensor incorporated by TRIDEC (Nether-lands) as part of their active trailer steering system [15]

In addition to being trailer-based the above systems are all lsquocontactrsquo type sensors(with the exception of the Orbisense sensor) This constrains the flexibility for use withvarious tractors and trailers and trailer types For tractor-based sensing a good casecan be made for non-contact sensing In the literature non-contact sensing solutionsfor this task have largely utilised state estimation methods (making use of other sensorinformation and models) or cameras

State observer methods have been demonstrated by Bouteldja et al [16] (using a stateobserver and an Extended Kalman Filter) Chu et al [17] and Ehlgen et al [18] Moderatemeasurement accuracies were obtained but only Ehlgen et alrsquos system was demonstratedin field tests achieving a maximum measurement error of 5

More recent work has focussed on the use of cameras arguably for their versatility andsignificant advances in the performance of computer vision algorithms In 2009 Schikoraet al [19] present a system for tractor semi-trailers comprising an upwards-facing cameraon the tractor unit viewing an encoder plate fitted to the underside of the semi-trailer(hence not strictly a tractor-based solution) Another system for rigid drawbar trailerswas also proposed using a rearward-facing infrared camera on the towing unit and aseries of infrared diodes fitted to the trailer (again not strictly tractor-based)

In 2013 Caup et al [20] presented a system for rigid drawbar trailers using a rear-facing camera The system requires the hitch location and drawbar length (a feature ofthe trailer) to be known and utilised a template-matching image processing method Thesystem was validated in field tests Also in 2013 Harris [21] presented another rear-facingcamera solution for tractor semi-trailer combinations Three image processing methodswere assessed including homography decomposition stereo vision (with the addition of asecond camera) and a template-matching method which showed improvements over thefirst two methods in a limited evaluation All three methods required the trailer front tobe planar (flat)

Fuchs et al [22] presented a similar rear-facing camera concept in 2014 which requiresmarkers in a known location to be fitted to the trailer together with knowledge ofvehicle geometry and camera location (and so not strictly trailer-based) With perfectknowledge of camera location a sub-1 accuracy was obtained in simulation but errorsrose significantly when uncertainty in camera location was incorporated In 2015 Fuchset al added a Kalman Filter to their system improving the accuracy [23]

In 2019 De Saxe and Cebon presented a further development on the rear-facing cam-era and template matching approach of Harris [24] incorporating an Unscented KalmanFilter to smooth the noisy template-matching measurements The system relied on someknowledge of the trailer geometry (making the solution not fully tractor-based) but sen-sitivity to this was shown to be small High accuracy was demonstrated in simulationswhile field tests on a tractor semi-trailer showed reasonable accuracy compared to thestate-of-the-art with RMS errors of 08ndash18 A large contributing factor to the experi-mental errors was relative pitch and roll motion between tractor and trailer which wasnot included in the simulations

As a benchmark for the current research results from the above literature were inves-tigated to extract the maximum measurement errors εmax demonstrated and maximum

3

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

articulation angles assessed (Γmax) RMS errors would arguably serve as a better bench-mark but are not easily available in all the studies It is clear from where simulationtests have been followed by full-scale testing that simulation results are a relatively poorindication of performance It is hence crucial to carry out full-scale testing

12 Research gap and objective

The rear-facing camera configuration coupled with a suitable computer vision algorithmis the clear candidate for further development given its versatility scope for being en-tirely tractor-based and the prevalence of powerful modern computer vision algorithmsThe described vision-based methods were shown to be limited to planar trailers withthe exception of [20] which was limited to drawbar-type trailers Further they were notdemonstrated to be compatible with two or more articulation pointsmdashan important con-sideration for LCVsmdashor not strictly tractor-based requiring known markers or knowledgeof trailer dimensions

For this work it was deemed fundamental to develop a system which was independentof trailer dimensions type or planarity such that the system could be compatible witha wide range of trailer types in use today including box trailers flat-bed trailers tankertrailer car-transporters etc without modification Given the prevalence of LCVs with twoor more articulation points it was also deemed important for the system to be adaptablefor the measurement of multiple articulation angles

In summary the objective of this research was to develop a new camera-based systemwhich

(1) is entirely tractor-based with no reliance on measurements from or geometric infor-mation about the trailer

(2) does not rely on the existence of known visual features on the trailer(3) is compatible with planar and non-planar trailer shapes(4) is compatible with two or more articulation points and(5) offers acceptable measurement accuracy for control applications in the order of 1

degree and(6) measures articulation angles up to 90 degrees

2 System overview

Based on previous work a rear-facing camera configuration was adopted The camerawas mounted to the tractor unit and faced the front of the trailer This is shown inFigure 2 The example of a single-articulation tractor semi-trailer vehicle combination isdepicted but the system will be shown later to be compatible with other combinationtypes

21 PTAM-based image processing

For the image processing algorithm a Simultaneous Localisation And Mapping orlsquoSLAMrsquo approach was adopted Primarily adopted in robotics applications SLAM per-tains to the problem of simultaneously determining the position and orientation of amoving agent and constructing a map of its three-dimensional environment This is sim-ilar to the problem of trailer motion sensing in this case the agent and environment areinterchangeable with the tractor and trailer respectively The objective is to determine

4

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 2 Rear-facing camera arrangement (shown for the case of a tractor semi-trailercombination)

the orientation of the trailer relative to the tractor and to build a map of the trailerto ensure compatibility with various planar and non-planar trailer shapes This inter-changeability is possible provided the trailer occupies the majority of the camera field ofview

Many solutions to the camera-based or visual SLAM problem (vSLAM) exist someof which are optimised for single cameras multiple cameras or other sensor inputs andinclude FastSLAM [25] FastSLAM 20 [26] Graph-based SLAM [27] Topological SLAM[28] LSD-SLAM [29] ORB-SLAM [30] and ORB-SLAM2 [31]

For this work the Parallel Tracking And Mapping (PTAM) algorithm by Klein andMurray [10] was adopted PTAM is a novel and highly efficient implementation of mono-camera vSLAM in which the localisation and mapping processing tasks are formulated inparallel The PTAM algorithm has been optimised for localised as opposed to explorationapplications meaning that the system expects to operate with a single scene and not toconstantly move into new scenes requiring recurring map extensions Being optimised asa localised system PTAM has superior pose estimation efficiency and accuracy versusexploratory systems with obvious benefits for the articulation angle sensing task Thename derives from the parallel nature in which the localisation (tracking) and mappingtasks are carried out on two separate CPU threads

The algorithm as implemented for the articulation sensing task is summarised in Fig-ure 3 [32] For an unseen trailer initially the algorithm requires two images of the trailerfront to create the initial three-dimensional trailer map (creating a point cloud frommatched features in the stereo image pair) These are called lsquokeyframesrsquo In practicethese can be obtained by briefly manoeuvring the trailer to two offset orientations Thesystem must then be zeroed which is achieved through a brief straight driving manoeu-vre

The stereo initialisation step is important as this defines the initial map upon whichall pose updates are based A small translation of the scene between the two initialkeyframes is required to complete the stereo mapping process The exact size of thistranslation is not prescribed and does not need to be known For typical hand-heldcamera applications Klein and Murray arbitrarily assumed the size of this translation inworld co-ordinates to be 10 cm in order to generate the initial map using the five-pointalgorithm [33] For the trailer sensing application the exact magnitude is not known andcan vary meaning the resultant Cartesian map is only accurate up to a scale factor (ierelative dimensions and translations are accurate but the precise scale is unknown) Aswe are only concerned with rotations (ie articulation angle) this is of no consequence

Ongoing orientation measurement can be achieved though matching features in thecurrent image with features in the existing trailer map (tracking) FAST features (Fea-

5

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 3 Flowchart of the PTAM-derived articulation angle measurement method [32]

tures from Accelerated Segment Test) [34] are used for this (as well as in the stereoinitialisation) and are tracked between frames using a decaying velocity motion model(similar to an alpha-beta filter [35]) and a simple pinhole camera model A perspec-tive projection pin-hole camera model is used for this purpose This is summarised inEquation 1 [36 37]

w = K[

R T]Xξuξv

ξ

=

kufx 0 u0

0 kvfy v0

0 0 1

[R T] XYZ1

(1)

XY Z are the location of a feature point in world coordinates u and v are the pixelcoordinates of the feature in the image plane R and T are the 3 times 3 rotation matrixand 3 times 1 translation vector of the relative camera motion K is the camera calibrationmatrix containing the lsquointrinsicrsquo camera parameters fx and fy are the focal lengths inthe horizontal and vertical directions u0 and v0 represent the location of the opticalaxis ku and kv are scaling parameters for rectangular pixels and ξ is the scaling factor(unknown in this case due to the scale ambiguity of using a single camera)

Lens distortion is accounted for using the simplified FOV-model [38] In this case pixelcoordinates are assumed to be distorted by a purely radial distortion which is describedusing a single distortion coefficient s as follows (where XC YC ZC are the location of thefeature point in camera-centred coordinates)

[uv

]=

[u0

v0

]+

[fx 00 fy

]rprime

r

[XcZc

YcZc

](2)

6

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

where

r =

radicX2

c + Y 2c

Z2c

rprime =1

sarctan

(2r tan

s

2

)(3)

In parallel to the tracking thread the mapping thread checks if new features (andhence potential new keyframes) are evident This would happen in practise if the trailerangle increased to a point where a new side of the trailer was seen for the first time (ifnot captured in the initial two keyframes) If a new keyframe is detected it is added tothe existing map and the map is updated If no new keyframes are detected the existingmap is refined through bundle adjustment [39]

22 Articulation angle extraction

The C++ source code for PTAM is open source [40] and was modified for application tothe articulation sensing task The algorithm provides two primary outputs a 3-D map offeature point locations and the camera pose in the form of a 3 times 1 translation vector Tand a 3times3 rotation matrix R (see Equation 1) During stereo initialisation the rotationmatrix is initialised relative to a dominant plane found in the first two keyframes and thetranslation vector represents the motion of the camera relative to its original location

Of interest here is the rotation matrix and particularly the yaw angle of the camerarelative to the observed scene (ie the trailer) In order to extract the articulation anglethe rotation matrix is decomposed into a combination of sequential rotations about eachaxis known as Euler angles roll (φ) yaw (ψ) and pitch (θ) This is summarised asfollows

R =

R11 R12 R13

R21 R22 R23

R31 R32 R33

= Rz(φ)Ry(ψ)Rx(θ) (4)

where

Rz(φ) =

cosφ minus sinφ 0sinφ cosφ 0

0 0 1

Ry(ψ) =

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

Rx(θ) =

1 0 00 cos θ minus sin θ0 sin θ cos θ

These Euler angles may be extracted from the rotation matrix using a combination of

the above definitions and logic to reject multiple solutions The method of [41] was usedfor this purpose These Euler angles are in the camera co-ordinate frame however and

7

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

must be translated into the trailer coordinate frame as followsφψθ

trailer

=

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

φψθ

camera

(5)

The articulation angle is then

Γ = ψtrailer (6)

23 Implementation

The modified PTAM algorithm was implemented in C++ on a 64-bit Linux laptopcomputer with a dual-core 24GHz Core i3 processor and 4GB of RAM Steady frameprocessing rates of 20 fps were achieved The built-in camera calibration tool availablewith PTAM was used to calibrate cameras

3 Simulations

31 Vehicle model

As an initial validation a simple 3D computer model of a tractor semi-trailer was createdin Autodesk Inventor with a single degree of freedom for articulation angle The modelis shown in Figure 2 A virtual camera was fixed behind the tractor cab as shown Thisfunctions as a simple lsquopin-holersquo camera with no distortion Photo-realistic visual texturewas added to the trailer based on an available test unit owned by the Cambridge VehicleDynamics Consortium (CVDC)

Representative articulation angle signals were generated using a three degree-of-freedom lsquobicycle modelrsquo of a tractor semi-trailer vehicle combination (More details canbe found in [24]) The model was subjected to a square wave steer input yielding anapproximately sinusoidal articulation angle signal The articulation angle signal was thenfed into the CAD model to generate sequences of image data from the virtual camerarepresentative of the articulation angle signals

32 Results

Two tests were carried out one reaching a maximum articulation angle of 50 andanother reaching 90 The 90 case served to test performance when the side of the trailercame into view forcing the need for new keyframes to be captured Results are shownin Figure 4 Γlim represents the angle beyond which the front of the trailer disappearsfrom view and only the trailer side is visible (around 70)

In both tests the errors increase approximately in line with increasing articulationangle potentially due to the increasing oblique camera view relative to where the stereoinitialisation took place The maximum errors do not align perfectly with maximumarticulation angles but rather seem to peak at around plusmn30 This may be a result ofvariations in the initialisation process The magnitude of errors may also be dependanton the keyframes used in initialisation No obvious degradation of accuracy is evidentbeyond Γlim Accuracy is good relative to the the published literature while the practicalshortcomings have been overcome

8

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 4: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

articulation angles assessed (Γmax) RMS errors would arguably serve as a better bench-mark but are not easily available in all the studies It is clear from where simulationtests have been followed by full-scale testing that simulation results are a relatively poorindication of performance It is hence crucial to carry out full-scale testing

12 Research gap and objective

The rear-facing camera configuration coupled with a suitable computer vision algorithmis the clear candidate for further development given its versatility scope for being en-tirely tractor-based and the prevalence of powerful modern computer vision algorithmsThe described vision-based methods were shown to be limited to planar trailers withthe exception of [20] which was limited to drawbar-type trailers Further they were notdemonstrated to be compatible with two or more articulation pointsmdashan important con-sideration for LCVsmdashor not strictly tractor-based requiring known markers or knowledgeof trailer dimensions

For this work it was deemed fundamental to develop a system which was independentof trailer dimensions type or planarity such that the system could be compatible witha wide range of trailer types in use today including box trailers flat-bed trailers tankertrailer car-transporters etc without modification Given the prevalence of LCVs with twoor more articulation points it was also deemed important for the system to be adaptablefor the measurement of multiple articulation angles

In summary the objective of this research was to develop a new camera-based systemwhich

(1) is entirely tractor-based with no reliance on measurements from or geometric infor-mation about the trailer

(2) does not rely on the existence of known visual features on the trailer(3) is compatible with planar and non-planar trailer shapes(4) is compatible with two or more articulation points and(5) offers acceptable measurement accuracy for control applications in the order of 1

degree and(6) measures articulation angles up to 90 degrees

2 System overview

Based on previous work a rear-facing camera configuration was adopted The camerawas mounted to the tractor unit and faced the front of the trailer This is shown inFigure 2 The example of a single-articulation tractor semi-trailer vehicle combination isdepicted but the system will be shown later to be compatible with other combinationtypes

21 PTAM-based image processing

For the image processing algorithm a Simultaneous Localisation And Mapping orlsquoSLAMrsquo approach was adopted Primarily adopted in robotics applications SLAM per-tains to the problem of simultaneously determining the position and orientation of amoving agent and constructing a map of its three-dimensional environment This is sim-ilar to the problem of trailer motion sensing in this case the agent and environment areinterchangeable with the tractor and trailer respectively The objective is to determine

4

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 2 Rear-facing camera arrangement (shown for the case of a tractor semi-trailercombination)

the orientation of the trailer relative to the tractor and to build a map of the trailerto ensure compatibility with various planar and non-planar trailer shapes This inter-changeability is possible provided the trailer occupies the majority of the camera field ofview

Many solutions to the camera-based or visual SLAM problem (vSLAM) exist someof which are optimised for single cameras multiple cameras or other sensor inputs andinclude FastSLAM [25] FastSLAM 20 [26] Graph-based SLAM [27] Topological SLAM[28] LSD-SLAM [29] ORB-SLAM [30] and ORB-SLAM2 [31]

For this work the Parallel Tracking And Mapping (PTAM) algorithm by Klein andMurray [10] was adopted PTAM is a novel and highly efficient implementation of mono-camera vSLAM in which the localisation and mapping processing tasks are formulated inparallel The PTAM algorithm has been optimised for localised as opposed to explorationapplications meaning that the system expects to operate with a single scene and not toconstantly move into new scenes requiring recurring map extensions Being optimised asa localised system PTAM has superior pose estimation efficiency and accuracy versusexploratory systems with obvious benefits for the articulation angle sensing task Thename derives from the parallel nature in which the localisation (tracking) and mappingtasks are carried out on two separate CPU threads

The algorithm as implemented for the articulation sensing task is summarised in Fig-ure 3 [32] For an unseen trailer initially the algorithm requires two images of the trailerfront to create the initial three-dimensional trailer map (creating a point cloud frommatched features in the stereo image pair) These are called lsquokeyframesrsquo In practicethese can be obtained by briefly manoeuvring the trailer to two offset orientations Thesystem must then be zeroed which is achieved through a brief straight driving manoeu-vre

The stereo initialisation step is important as this defines the initial map upon whichall pose updates are based A small translation of the scene between the two initialkeyframes is required to complete the stereo mapping process The exact size of thistranslation is not prescribed and does not need to be known For typical hand-heldcamera applications Klein and Murray arbitrarily assumed the size of this translation inworld co-ordinates to be 10 cm in order to generate the initial map using the five-pointalgorithm [33] For the trailer sensing application the exact magnitude is not known andcan vary meaning the resultant Cartesian map is only accurate up to a scale factor (ierelative dimensions and translations are accurate but the precise scale is unknown) Aswe are only concerned with rotations (ie articulation angle) this is of no consequence

Ongoing orientation measurement can be achieved though matching features in thecurrent image with features in the existing trailer map (tracking) FAST features (Fea-

5

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 3 Flowchart of the PTAM-derived articulation angle measurement method [32]

tures from Accelerated Segment Test) [34] are used for this (as well as in the stereoinitialisation) and are tracked between frames using a decaying velocity motion model(similar to an alpha-beta filter [35]) and a simple pinhole camera model A perspec-tive projection pin-hole camera model is used for this purpose This is summarised inEquation 1 [36 37]

w = K[

R T]Xξuξv

ξ

=

kufx 0 u0

0 kvfy v0

0 0 1

[R T] XYZ1

(1)

XY Z are the location of a feature point in world coordinates u and v are the pixelcoordinates of the feature in the image plane R and T are the 3 times 3 rotation matrixand 3 times 1 translation vector of the relative camera motion K is the camera calibrationmatrix containing the lsquointrinsicrsquo camera parameters fx and fy are the focal lengths inthe horizontal and vertical directions u0 and v0 represent the location of the opticalaxis ku and kv are scaling parameters for rectangular pixels and ξ is the scaling factor(unknown in this case due to the scale ambiguity of using a single camera)

Lens distortion is accounted for using the simplified FOV-model [38] In this case pixelcoordinates are assumed to be distorted by a purely radial distortion which is describedusing a single distortion coefficient s as follows (where XC YC ZC are the location of thefeature point in camera-centred coordinates)

[uv

]=

[u0

v0

]+

[fx 00 fy

]rprime

r

[XcZc

YcZc

](2)

6

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

where

r =

radicX2

c + Y 2c

Z2c

rprime =1

sarctan

(2r tan

s

2

)(3)

In parallel to the tracking thread the mapping thread checks if new features (andhence potential new keyframes) are evident This would happen in practise if the trailerangle increased to a point where a new side of the trailer was seen for the first time (ifnot captured in the initial two keyframes) If a new keyframe is detected it is added tothe existing map and the map is updated If no new keyframes are detected the existingmap is refined through bundle adjustment [39]

22 Articulation angle extraction

The C++ source code for PTAM is open source [40] and was modified for application tothe articulation sensing task The algorithm provides two primary outputs a 3-D map offeature point locations and the camera pose in the form of a 3 times 1 translation vector Tand a 3times3 rotation matrix R (see Equation 1) During stereo initialisation the rotationmatrix is initialised relative to a dominant plane found in the first two keyframes and thetranslation vector represents the motion of the camera relative to its original location

Of interest here is the rotation matrix and particularly the yaw angle of the camerarelative to the observed scene (ie the trailer) In order to extract the articulation anglethe rotation matrix is decomposed into a combination of sequential rotations about eachaxis known as Euler angles roll (φ) yaw (ψ) and pitch (θ) This is summarised asfollows

R =

R11 R12 R13

R21 R22 R23

R31 R32 R33

= Rz(φ)Ry(ψ)Rx(θ) (4)

where

Rz(φ) =

cosφ minus sinφ 0sinφ cosφ 0

0 0 1

Ry(ψ) =

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

Rx(θ) =

1 0 00 cos θ minus sin θ0 sin θ cos θ

These Euler angles may be extracted from the rotation matrix using a combination of

the above definitions and logic to reject multiple solutions The method of [41] was usedfor this purpose These Euler angles are in the camera co-ordinate frame however and

7

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

must be translated into the trailer coordinate frame as followsφψθ

trailer

=

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

φψθ

camera

(5)

The articulation angle is then

Γ = ψtrailer (6)

23 Implementation

The modified PTAM algorithm was implemented in C++ on a 64-bit Linux laptopcomputer with a dual-core 24GHz Core i3 processor and 4GB of RAM Steady frameprocessing rates of 20 fps were achieved The built-in camera calibration tool availablewith PTAM was used to calibrate cameras

3 Simulations

31 Vehicle model

As an initial validation a simple 3D computer model of a tractor semi-trailer was createdin Autodesk Inventor with a single degree of freedom for articulation angle The modelis shown in Figure 2 A virtual camera was fixed behind the tractor cab as shown Thisfunctions as a simple lsquopin-holersquo camera with no distortion Photo-realistic visual texturewas added to the trailer based on an available test unit owned by the Cambridge VehicleDynamics Consortium (CVDC)

Representative articulation angle signals were generated using a three degree-of-freedom lsquobicycle modelrsquo of a tractor semi-trailer vehicle combination (More details canbe found in [24]) The model was subjected to a square wave steer input yielding anapproximately sinusoidal articulation angle signal The articulation angle signal was thenfed into the CAD model to generate sequences of image data from the virtual camerarepresentative of the articulation angle signals

32 Results

Two tests were carried out one reaching a maximum articulation angle of 50 andanother reaching 90 The 90 case served to test performance when the side of the trailercame into view forcing the need for new keyframes to be captured Results are shownin Figure 4 Γlim represents the angle beyond which the front of the trailer disappearsfrom view and only the trailer side is visible (around 70)

In both tests the errors increase approximately in line with increasing articulationangle potentially due to the increasing oblique camera view relative to where the stereoinitialisation took place The maximum errors do not align perfectly with maximumarticulation angles but rather seem to peak at around plusmn30 This may be a result ofvariations in the initialisation process The magnitude of errors may also be dependanton the keyframes used in initialisation No obvious degradation of accuracy is evidentbeyond Γlim Accuracy is good relative to the the published literature while the practicalshortcomings have been overcome

8

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 5: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 2 Rear-facing camera arrangement (shown for the case of a tractor semi-trailercombination)

the orientation of the trailer relative to the tractor and to build a map of the trailerto ensure compatibility with various planar and non-planar trailer shapes This inter-changeability is possible provided the trailer occupies the majority of the camera field ofview

Many solutions to the camera-based or visual SLAM problem (vSLAM) exist someof which are optimised for single cameras multiple cameras or other sensor inputs andinclude FastSLAM [25] FastSLAM 20 [26] Graph-based SLAM [27] Topological SLAM[28] LSD-SLAM [29] ORB-SLAM [30] and ORB-SLAM2 [31]

For this work the Parallel Tracking And Mapping (PTAM) algorithm by Klein andMurray [10] was adopted PTAM is a novel and highly efficient implementation of mono-camera vSLAM in which the localisation and mapping processing tasks are formulated inparallel The PTAM algorithm has been optimised for localised as opposed to explorationapplications meaning that the system expects to operate with a single scene and not toconstantly move into new scenes requiring recurring map extensions Being optimised asa localised system PTAM has superior pose estimation efficiency and accuracy versusexploratory systems with obvious benefits for the articulation angle sensing task Thename derives from the parallel nature in which the localisation (tracking) and mappingtasks are carried out on two separate CPU threads

The algorithm as implemented for the articulation sensing task is summarised in Fig-ure 3 [32] For an unseen trailer initially the algorithm requires two images of the trailerfront to create the initial three-dimensional trailer map (creating a point cloud frommatched features in the stereo image pair) These are called lsquokeyframesrsquo In practicethese can be obtained by briefly manoeuvring the trailer to two offset orientations Thesystem must then be zeroed which is achieved through a brief straight driving manoeu-vre

The stereo initialisation step is important as this defines the initial map upon whichall pose updates are based A small translation of the scene between the two initialkeyframes is required to complete the stereo mapping process The exact size of thistranslation is not prescribed and does not need to be known For typical hand-heldcamera applications Klein and Murray arbitrarily assumed the size of this translation inworld co-ordinates to be 10 cm in order to generate the initial map using the five-pointalgorithm [33] For the trailer sensing application the exact magnitude is not known andcan vary meaning the resultant Cartesian map is only accurate up to a scale factor (ierelative dimensions and translations are accurate but the precise scale is unknown) Aswe are only concerned with rotations (ie articulation angle) this is of no consequence

Ongoing orientation measurement can be achieved though matching features in thecurrent image with features in the existing trailer map (tracking) FAST features (Fea-

5

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 3 Flowchart of the PTAM-derived articulation angle measurement method [32]

tures from Accelerated Segment Test) [34] are used for this (as well as in the stereoinitialisation) and are tracked between frames using a decaying velocity motion model(similar to an alpha-beta filter [35]) and a simple pinhole camera model A perspec-tive projection pin-hole camera model is used for this purpose This is summarised inEquation 1 [36 37]

w = K[

R T]Xξuξv

ξ

=

kufx 0 u0

0 kvfy v0

0 0 1

[R T] XYZ1

(1)

XY Z are the location of a feature point in world coordinates u and v are the pixelcoordinates of the feature in the image plane R and T are the 3 times 3 rotation matrixand 3 times 1 translation vector of the relative camera motion K is the camera calibrationmatrix containing the lsquointrinsicrsquo camera parameters fx and fy are the focal lengths inthe horizontal and vertical directions u0 and v0 represent the location of the opticalaxis ku and kv are scaling parameters for rectangular pixels and ξ is the scaling factor(unknown in this case due to the scale ambiguity of using a single camera)

Lens distortion is accounted for using the simplified FOV-model [38] In this case pixelcoordinates are assumed to be distorted by a purely radial distortion which is describedusing a single distortion coefficient s as follows (where XC YC ZC are the location of thefeature point in camera-centred coordinates)

[uv

]=

[u0

v0

]+

[fx 00 fy

]rprime

r

[XcZc

YcZc

](2)

6

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

where

r =

radicX2

c + Y 2c

Z2c

rprime =1

sarctan

(2r tan

s

2

)(3)

In parallel to the tracking thread the mapping thread checks if new features (andhence potential new keyframes) are evident This would happen in practise if the trailerangle increased to a point where a new side of the trailer was seen for the first time (ifnot captured in the initial two keyframes) If a new keyframe is detected it is added tothe existing map and the map is updated If no new keyframes are detected the existingmap is refined through bundle adjustment [39]

22 Articulation angle extraction

The C++ source code for PTAM is open source [40] and was modified for application tothe articulation sensing task The algorithm provides two primary outputs a 3-D map offeature point locations and the camera pose in the form of a 3 times 1 translation vector Tand a 3times3 rotation matrix R (see Equation 1) During stereo initialisation the rotationmatrix is initialised relative to a dominant plane found in the first two keyframes and thetranslation vector represents the motion of the camera relative to its original location

Of interest here is the rotation matrix and particularly the yaw angle of the camerarelative to the observed scene (ie the trailer) In order to extract the articulation anglethe rotation matrix is decomposed into a combination of sequential rotations about eachaxis known as Euler angles roll (φ) yaw (ψ) and pitch (θ) This is summarised asfollows

R =

R11 R12 R13

R21 R22 R23

R31 R32 R33

= Rz(φ)Ry(ψ)Rx(θ) (4)

where

Rz(φ) =

cosφ minus sinφ 0sinφ cosφ 0

0 0 1

Ry(ψ) =

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

Rx(θ) =

1 0 00 cos θ minus sin θ0 sin θ cos θ

These Euler angles may be extracted from the rotation matrix using a combination of

the above definitions and logic to reject multiple solutions The method of [41] was usedfor this purpose These Euler angles are in the camera co-ordinate frame however and

7

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

must be translated into the trailer coordinate frame as followsφψθ

trailer

=

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

φψθ

camera

(5)

The articulation angle is then

Γ = ψtrailer (6)

23 Implementation

The modified PTAM algorithm was implemented in C++ on a 64-bit Linux laptopcomputer with a dual-core 24GHz Core i3 processor and 4GB of RAM Steady frameprocessing rates of 20 fps were achieved The built-in camera calibration tool availablewith PTAM was used to calibrate cameras

3 Simulations

31 Vehicle model

As an initial validation a simple 3D computer model of a tractor semi-trailer was createdin Autodesk Inventor with a single degree of freedom for articulation angle The modelis shown in Figure 2 A virtual camera was fixed behind the tractor cab as shown Thisfunctions as a simple lsquopin-holersquo camera with no distortion Photo-realistic visual texturewas added to the trailer based on an available test unit owned by the Cambridge VehicleDynamics Consortium (CVDC)

Representative articulation angle signals were generated using a three degree-of-freedom lsquobicycle modelrsquo of a tractor semi-trailer vehicle combination (More details canbe found in [24]) The model was subjected to a square wave steer input yielding anapproximately sinusoidal articulation angle signal The articulation angle signal was thenfed into the CAD model to generate sequences of image data from the virtual camerarepresentative of the articulation angle signals

32 Results

Two tests were carried out one reaching a maximum articulation angle of 50 andanother reaching 90 The 90 case served to test performance when the side of the trailercame into view forcing the need for new keyframes to be captured Results are shownin Figure 4 Γlim represents the angle beyond which the front of the trailer disappearsfrom view and only the trailer side is visible (around 70)

In both tests the errors increase approximately in line with increasing articulationangle potentially due to the increasing oblique camera view relative to where the stereoinitialisation took place The maximum errors do not align perfectly with maximumarticulation angles but rather seem to peak at around plusmn30 This may be a result ofvariations in the initialisation process The magnitude of errors may also be dependanton the keyframes used in initialisation No obvious degradation of accuracy is evidentbeyond Γlim Accuracy is good relative to the the published literature while the practicalshortcomings have been overcome

8

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 6: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 3 Flowchart of the PTAM-derived articulation angle measurement method [32]

tures from Accelerated Segment Test) [34] are used for this (as well as in the stereoinitialisation) and are tracked between frames using a decaying velocity motion model(similar to an alpha-beta filter [35]) and a simple pinhole camera model A perspec-tive projection pin-hole camera model is used for this purpose This is summarised inEquation 1 [36 37]

w = K[

R T]Xξuξv

ξ

=

kufx 0 u0

0 kvfy v0

0 0 1

[R T] XYZ1

(1)

XY Z are the location of a feature point in world coordinates u and v are the pixelcoordinates of the feature in the image plane R and T are the 3 times 3 rotation matrixand 3 times 1 translation vector of the relative camera motion K is the camera calibrationmatrix containing the lsquointrinsicrsquo camera parameters fx and fy are the focal lengths inthe horizontal and vertical directions u0 and v0 represent the location of the opticalaxis ku and kv are scaling parameters for rectangular pixels and ξ is the scaling factor(unknown in this case due to the scale ambiguity of using a single camera)

Lens distortion is accounted for using the simplified FOV-model [38] In this case pixelcoordinates are assumed to be distorted by a purely radial distortion which is describedusing a single distortion coefficient s as follows (where XC YC ZC are the location of thefeature point in camera-centred coordinates)

[uv

]=

[u0

v0

]+

[fx 00 fy

]rprime

r

[XcZc

YcZc

](2)

6

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

where

r =

radicX2

c + Y 2c

Z2c

rprime =1

sarctan

(2r tan

s

2

)(3)

In parallel to the tracking thread the mapping thread checks if new features (andhence potential new keyframes) are evident This would happen in practise if the trailerangle increased to a point where a new side of the trailer was seen for the first time (ifnot captured in the initial two keyframes) If a new keyframe is detected it is added tothe existing map and the map is updated If no new keyframes are detected the existingmap is refined through bundle adjustment [39]

22 Articulation angle extraction

The C++ source code for PTAM is open source [40] and was modified for application tothe articulation sensing task The algorithm provides two primary outputs a 3-D map offeature point locations and the camera pose in the form of a 3 times 1 translation vector Tand a 3times3 rotation matrix R (see Equation 1) During stereo initialisation the rotationmatrix is initialised relative to a dominant plane found in the first two keyframes and thetranslation vector represents the motion of the camera relative to its original location

Of interest here is the rotation matrix and particularly the yaw angle of the camerarelative to the observed scene (ie the trailer) In order to extract the articulation anglethe rotation matrix is decomposed into a combination of sequential rotations about eachaxis known as Euler angles roll (φ) yaw (ψ) and pitch (θ) This is summarised asfollows

R =

R11 R12 R13

R21 R22 R23

R31 R32 R33

= Rz(φ)Ry(ψ)Rx(θ) (4)

where

Rz(φ) =

cosφ minus sinφ 0sinφ cosφ 0

0 0 1

Ry(ψ) =

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

Rx(θ) =

1 0 00 cos θ minus sin θ0 sin θ cos θ

These Euler angles may be extracted from the rotation matrix using a combination of

the above definitions and logic to reject multiple solutions The method of [41] was usedfor this purpose These Euler angles are in the camera co-ordinate frame however and

7

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

must be translated into the trailer coordinate frame as followsφψθ

trailer

=

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

φψθ

camera

(5)

The articulation angle is then

Γ = ψtrailer (6)

23 Implementation

The modified PTAM algorithm was implemented in C++ on a 64-bit Linux laptopcomputer with a dual-core 24GHz Core i3 processor and 4GB of RAM Steady frameprocessing rates of 20 fps were achieved The built-in camera calibration tool availablewith PTAM was used to calibrate cameras

3 Simulations

31 Vehicle model

As an initial validation a simple 3D computer model of a tractor semi-trailer was createdin Autodesk Inventor with a single degree of freedom for articulation angle The modelis shown in Figure 2 A virtual camera was fixed behind the tractor cab as shown Thisfunctions as a simple lsquopin-holersquo camera with no distortion Photo-realistic visual texturewas added to the trailer based on an available test unit owned by the Cambridge VehicleDynamics Consortium (CVDC)

Representative articulation angle signals were generated using a three degree-of-freedom lsquobicycle modelrsquo of a tractor semi-trailer vehicle combination (More details canbe found in [24]) The model was subjected to a square wave steer input yielding anapproximately sinusoidal articulation angle signal The articulation angle signal was thenfed into the CAD model to generate sequences of image data from the virtual camerarepresentative of the articulation angle signals

32 Results

Two tests were carried out one reaching a maximum articulation angle of 50 andanother reaching 90 The 90 case served to test performance when the side of the trailercame into view forcing the need for new keyframes to be captured Results are shownin Figure 4 Γlim represents the angle beyond which the front of the trailer disappearsfrom view and only the trailer side is visible (around 70)

In both tests the errors increase approximately in line with increasing articulationangle potentially due to the increasing oblique camera view relative to where the stereoinitialisation took place The maximum errors do not align perfectly with maximumarticulation angles but rather seem to peak at around plusmn30 This may be a result ofvariations in the initialisation process The magnitude of errors may also be dependanton the keyframes used in initialisation No obvious degradation of accuracy is evidentbeyond Γlim Accuracy is good relative to the the published literature while the practicalshortcomings have been overcome

8

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 7: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

where

r =

radicX2

c + Y 2c

Z2c

rprime =1

sarctan

(2r tan

s

2

)(3)

In parallel to the tracking thread the mapping thread checks if new features (andhence potential new keyframes) are evident This would happen in practise if the trailerangle increased to a point where a new side of the trailer was seen for the first time (ifnot captured in the initial two keyframes) If a new keyframe is detected it is added tothe existing map and the map is updated If no new keyframes are detected the existingmap is refined through bundle adjustment [39]

22 Articulation angle extraction

The C++ source code for PTAM is open source [40] and was modified for application tothe articulation sensing task The algorithm provides two primary outputs a 3-D map offeature point locations and the camera pose in the form of a 3 times 1 translation vector Tand a 3times3 rotation matrix R (see Equation 1) During stereo initialisation the rotationmatrix is initialised relative to a dominant plane found in the first two keyframes and thetranslation vector represents the motion of the camera relative to its original location

Of interest here is the rotation matrix and particularly the yaw angle of the camerarelative to the observed scene (ie the trailer) In order to extract the articulation anglethe rotation matrix is decomposed into a combination of sequential rotations about eachaxis known as Euler angles roll (φ) yaw (ψ) and pitch (θ) This is summarised asfollows

R =

R11 R12 R13

R21 R22 R23

R31 R32 R33

= Rz(φ)Ry(ψ)Rx(θ) (4)

where

Rz(φ) =

cosφ minus sinφ 0sinφ cosφ 0

0 0 1

Ry(ψ) =

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

Rx(θ) =

1 0 00 cos θ minus sin θ0 sin θ cos θ

These Euler angles may be extracted from the rotation matrix using a combination of

the above definitions and logic to reject multiple solutions The method of [41] was usedfor this purpose These Euler angles are in the camera co-ordinate frame however and

7

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

must be translated into the trailer coordinate frame as followsφψθ

trailer

=

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

φψθ

camera

(5)

The articulation angle is then

Γ = ψtrailer (6)

23 Implementation

The modified PTAM algorithm was implemented in C++ on a 64-bit Linux laptopcomputer with a dual-core 24GHz Core i3 processor and 4GB of RAM Steady frameprocessing rates of 20 fps were achieved The built-in camera calibration tool availablewith PTAM was used to calibrate cameras

3 Simulations

31 Vehicle model

As an initial validation a simple 3D computer model of a tractor semi-trailer was createdin Autodesk Inventor with a single degree of freedom for articulation angle The modelis shown in Figure 2 A virtual camera was fixed behind the tractor cab as shown Thisfunctions as a simple lsquopin-holersquo camera with no distortion Photo-realistic visual texturewas added to the trailer based on an available test unit owned by the Cambridge VehicleDynamics Consortium (CVDC)

Representative articulation angle signals were generated using a three degree-of-freedom lsquobicycle modelrsquo of a tractor semi-trailer vehicle combination (More details canbe found in [24]) The model was subjected to a square wave steer input yielding anapproximately sinusoidal articulation angle signal The articulation angle signal was thenfed into the CAD model to generate sequences of image data from the virtual camerarepresentative of the articulation angle signals

32 Results

Two tests were carried out one reaching a maximum articulation angle of 50 andanother reaching 90 The 90 case served to test performance when the side of the trailercame into view forcing the need for new keyframes to be captured Results are shownin Figure 4 Γlim represents the angle beyond which the front of the trailer disappearsfrom view and only the trailer side is visible (around 70)

In both tests the errors increase approximately in line with increasing articulationangle potentially due to the increasing oblique camera view relative to where the stereoinitialisation took place The maximum errors do not align perfectly with maximumarticulation angles but rather seem to peak at around plusmn30 This may be a result ofvariations in the initialisation process The magnitude of errors may also be dependanton the keyframes used in initialisation No obvious degradation of accuracy is evidentbeyond Γlim Accuracy is good relative to the the published literature while the practicalshortcomings have been overcome

8

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 8: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

must be translated into the trailer coordinate frame as followsφψθ

trailer

=

cosψ 0 sinψ0 1 0

minus sinψ 0 cosψ

φψθ

camera

(5)

The articulation angle is then

Γ = ψtrailer (6)

23 Implementation

The modified PTAM algorithm was implemented in C++ on a 64-bit Linux laptopcomputer with a dual-core 24GHz Core i3 processor and 4GB of RAM Steady frameprocessing rates of 20 fps were achieved The built-in camera calibration tool availablewith PTAM was used to calibrate cameras

3 Simulations

31 Vehicle model

As an initial validation a simple 3D computer model of a tractor semi-trailer was createdin Autodesk Inventor with a single degree of freedom for articulation angle The modelis shown in Figure 2 A virtual camera was fixed behind the tractor cab as shown Thisfunctions as a simple lsquopin-holersquo camera with no distortion Photo-realistic visual texturewas added to the trailer based on an available test unit owned by the Cambridge VehicleDynamics Consortium (CVDC)

Representative articulation angle signals were generated using a three degree-of-freedom lsquobicycle modelrsquo of a tractor semi-trailer vehicle combination (More details canbe found in [24]) The model was subjected to a square wave steer input yielding anapproximately sinusoidal articulation angle signal The articulation angle signal was thenfed into the CAD model to generate sequences of image data from the virtual camerarepresentative of the articulation angle signals

32 Results

Two tests were carried out one reaching a maximum articulation angle of 50 andanother reaching 90 The 90 case served to test performance when the side of the trailercame into view forcing the need for new keyframes to be captured Results are shownin Figure 4 Γlim represents the angle beyond which the front of the trailer disappearsfrom view and only the trailer side is visible (around 70)

In both tests the errors increase approximately in line with increasing articulationangle potentially due to the increasing oblique camera view relative to where the stereoinitialisation took place The maximum errors do not align perfectly with maximumarticulation angles but rather seem to peak at around plusmn30 This may be a result ofvariations in the initialisation process The magnitude of errors may also be dependanton the keyframes used in initialisation No obvious degradation of accuracy is evidentbeyond Γlim Accuracy is good relative to the the published literature while the practicalshortcomings have been overcome

8

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 9: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

-90

-60

-30

0

30

60

90A

rtic

ulat

ion

angl

e (deg

)

0 10 20 30 40 50 60 70Time (s)

-2-1012

Err

or (deg)

(a) 50 test

Art

icul

atio

n an

gle

(deg)

-90

-60

-30

0

30

60

90

Ground truthPTAM

Time (s)0 10 20 30 40 50 60 70

Err

or (deg)

-2-1012

- lim

+ lim

(b) 90 test

Figure 4 Example simulation results

(a) Front (b) Positive Γ (c) Negative Γ

Figure 5 Simulated semi-trailer feature map

Figure 5 shows details of the features tracked during the simulation Features on boththe trailer sides were added to the map successfully as they become visible at highervalues of Γ In the third frame it can be seen that the front of the trailer has rotatedout of view at high negative Γ This is in accordance with minusΓlim being exceeded as seenin Figure 4

4 Field tests Tractor semi-trailer

41 Vehicle and instrumentation

Initial full-scale vehicle tests were carried out on a tractor semi-trailer combinationwith one articulation point The test vehicle and instrumentation used is illustrated inFigure 6 A Point Grey Flea3 USB 30 camera was fitted behind the tractor cabin witha vari-focus lens set to f = 28 mm Images were captured in greyscale at 640times 480 andat 20 fps The lsquoground truthrsquo articulation angle measurement came from a VSE rotarypotentiometer articulation angle sensor [12] fitted to the trailer kingpin

Testing was carried out at Bourn airfield near Cambridge Two trailer front faceswere considered a planar front (a predominantly flat surface) and a non-planar frontcomprising a mock refrigeration unit (a common addition to trailer fronts)

Six tests with the planar trailer front were carried out comprising three periodic lsquosquarewaversquo steering tests with articulation angles up to 30 (denoted lsquoper30rsquo) and three lsquogen-eralrsquo driving manoeuvres (denoted lsquogenrsquo) Six tests with the non-planar trailer front (re-frigeration unit) were carried out The maximum articulation angle was also increased

9

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 10: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Steer sensor

Steer sensor

xPC host PC

Linux PC

camera

Trailer data logger

LaptopAnalogue

USB 30

CAN bus

Ethernet

Signal key

Tractor data loggerTractor speed

sensor

TractorSemi-trailer

VSE

sensor

Figure 6 Tractor semi-trailer test vehicle and instrumentation [24]

two periodic tests up to 30 (lsquoper30 3drsquo) two periodic tests up to 50 (lsquoper50 3drsquo) andtwo general tests up to 55 (lsquogen 3drsquo) Vehicle speed was approximately 5 kmh for thestep steer tests and in the range 0ndash10 kmh for the general driving tests

42 Initialisation and zeroing

An example of the stereo initialisation and zeroing procedure is given in Figure 7 show-ing a time history of PTAM and ground truth measurements As some lateral movementof the semi-trailer or drawbar begins the first keyframe is captured Once sufficient ad-ditional lateral motion has occurred the second keyframe is captured (shown at approxi-mately 12 seconds) defining the initial stereo map Hereafter tracking and measurementtakes place but the co-ordinate system is not necessarily aligned with the vehicle as isevident in the measurement bias

At approximately 72 seconds when it was observed that the trailer was straight azeroing command was sent to the algorithm applying a zeroing correction to all subse-quent measurements At this point the instantaneous rotation matrix R is recorded (seeEquation 4) Subsequent rotation matrices were post-multiplied by this reference matrixto adjust the co-ordinate frame accordingly before performing Euler decomposition toextract the articulation angle

43 Results

Example measurement time histories are shown in Figure 8 including tests with a planarand non-planar trailer front The non-planar tests at larger Γ exhibited greater errorsthan the planar tests at smaller Γ it appears that there is no sensitivity to the planarity ofthe trailer Measurements also appear to be robust to transient pitch and roll variationsIn the non-planar tests keyframes from the trailer sides are added with no discernibleeffect on performance

10

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 11: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Figure 7 Stereo initialisation and zeroing in field tests

-40

-20

0

20

40

Art

icul

atio

n an

gle

(deg)

Ground truthPTAM

10 20 30 40 50 60Time (s)

-5

0

5

Err

or (deg)

(a) per30 A test (planar)

-40

-20

0

20

40A

rtic

ulat

ion

angl

e (deg

)

Ground truthPTAM

30 40 50 60 70 80 90Time (s)

-505

Err

or (deg)

(b) gen A test (planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(c) per30 3d A test (non-planar)

Art

icul

atio

n an

gle

(deg)

-60

-40

-20

0

20

40

60

Ground truthPTAM

Time (s)0 50 100 150

Err

or (deg)

-505

(d) gen 3d B test (non-planar)

Figure 8 Vehicle test time histories

Feature-tracking details are provided in Figure 9 both planar and non-planar tests Forthe non-planar test reaching 55 it can be seen that features on the side of the trailerare successfully detected and tracked when the front face is no longer visible

5 Field tests Truck and full-trailer

In the previous section the PTAM method was shown to be effective on tractor semi-trailer combination These combinations are common throughout the world and so it

11

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 12: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Planar front (b) Non-planar front (c) Non-planar side

Figure 9 Semi-trailer feature maps

is important that the articulation angle sensor is compatible with them As discussedpreviously however it is important to demonstrate compatibility with LCVs with morethan one articulation point as well These are increasing in use in Europe and are alreadycommon in countries including Sweden Australia New Zealand and South Africa In thissection the applicability of the sensor concept is extended to a vehicle combination com-prising a rigid truck converter dolly and semi-trailer These have been widely adopted inSweden and Finland and are hence known as the lsquoNordic combinationrsquo typically 2525 min length This is depicted in Figure 1 (the fifth vehicle)

In order for the sensor system to measure two articulation angles still using a singlecamera both articulating trailer bodies must be visible in the camerarsquos field of view Bothregions of interest can then be processed independently using the same PTAM-methodas before The only modifications required are the positioning of the camera to ensurethe correct field-of-view and the separation of the view into separate regions of interestfor independent processing

51 Vehicle and instrumentation

A Nordic combination vehicle was made available for testing by Volvo Group TrucksTechnology (GTT) Sweden The combination consisted of a rigid truck converter dollyand semi-trailer The available truck was bare of any superstructure but the additionof typical superstructures would not be expected to obscure the camera field of viewThe semi-trailer had a planar front though as demonstrated previously this is not arequirement for accurate operation of the system The vehicle and instrumentation isillustrated in Figure 10 The camera was mounted to the chassis of the truck unit nearthe location of the existing reverse-aid camera already fitted (the location of which wouldalso suffice)

The same Point Grey Flea3 USB camera with Fujinon lens used previously was fixedto the rear of the truck chassis as shown approximately along the longitudinal axis of thetruck The camera was calibrated using the built-in PTAM calibrator as before Fromthis location the field of view of the camera included both the drawbar of the dolly andthe front of the semi-trailer Regions are interest in the images were chosen manuallyand are illustrated in Figure 11 The sizes of the partitions were 440times 270 and 560times 180pixels for the semi-trailer and drawbar respectively and were centred laterally

Testing was carried out at the Hallered proving ground near Gothenburg Manoeuvreswere carried out around the trailer storage yard next to a workshop which providedthe visual scenery and manoeuvres representative of truck loading and unloading areasArbitrary visual markers were added to the otherwise bare trailer front using piecesof duct tape which were then removed during testing to compare performance Six

12

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 13: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Laptop

USB 30

CANbus

Analogue

Key

dSpace

Autobox

VSE

sensor

Pintle hitch

sensor

Camera

Rigid truckDollySemi-trailer

Figure 10 Nordic combination test vehicle and instrumentation (dimensions in mm)

Figure 11 Image regions of interest used for processing (zero and non-zero articulationangles shown)

manoeuvres were carried out a left turn loop (lsquoleft turnrsquo) a right turn loop (lsquorightturnrsquo) two arbitrary routes around the yard with both left and right turns (lsquoarbitrary Arsquoand lsquoarbitrary Brsquo) a short manoeuvre over an uneven unpaved surface (lsquounevenrsquo) Thelsquoleft turnrsquo manoeuvre was then repeated with the trailer markers removed (lsquoleft turn nomarkersrsquo) for comparison of performance with and without markers Three runs of eachmanoeuvre were carried out

The lsquoground truthrsquo articulation angle measurements were obtained via a VSE sensorat the fifth wheel and a custom sensor provided by Volvo at the pintle hitch Stereoinitialisation and zeroing was carried out in the same manner as the previous tests withthe semi-trailer and dolly regions of interest being processed separately

13

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 14: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(a) Left turn

10

0

10

20

30

40

50

60

Artic

ang

le (d

eg)

PTAMGround truth

0 10 20 30 40 50 60 70 80 90 100 110Time (s)

5

0

5

Erro

r (de

g)

(b) Left turn no markers

Figure 12 Time histories semi-trailer

52 Results

Example time histories of articulation angle measurements are shown in Figure 12 (semi-trailer angle left turn marker and no marker tests) PTAM measures the total relativepose between camera and environment and so in the case of the semi-trailer this is thesum of drawbar and semi-trailer articulation angles Therefore in this case the sum ofpintle hitch and VSE sensor measurements were used as the ground truth

The manoeuvres conducted provided a large variation in background scenery andthe results show no discernible sensitivity to this There is also no clear difference inperformance between the left turn results and left turn with no markers indicatingthat the markers are not necessary and that the system performs well with visuallybare trailers Unusually high errors (not in line with the other tests) are evident in themanoeuvres on uneven ground These tests were intended to demonstrate sensitivity totransient pitch and roll motion of the trailer Anecdotal evidence gained by observing thebumpy vehicle motion in the video sequences and comparing the results with smootherruns suggests that there is little if any sensitivity to this However zeroing these tests wasdifficult given that there was not an obvious period of straight-line driving This likelyresulted in an inaccurate zeroing of articulation angle yielding a small bias combined witha potentially non-zero pitch and or roll yielding an inaccurate pitch and roll componentto R (See Equation 4)

Example initialised feature maps are shown in Figure 13 for the semi-trailer (markedand unmarked) and the drawbar It is clear that the markers introduced additionalfeature points compared with the unmarked semi-trailer but that the unmarked semi-trailer still provided sufficient features for initialisation and tracking In both cases therewas a concentration of features around the service connections and along the bottomedge of the semi-trailer around rivets trailer name plates and trailer identification codeThe drawbar possessed sufficient natural features for initialisation and tracking in theform of edges bolts and connector points

It was evident during testing that a larger rotation was required during the drawbarstereo initialisation process If the motion was too small the initialisation would eitherfail or yield obviously erroneous andor erratic measurements This is due to the fact thatthe camera was located near the centre of rotation of the drawbar (at the pintle hitch)meaning that relative motion between the camera and observed body was dominated bya rotational element with little translation as required for the stereo initialisation Inpractice it would be straight-forward to detect a failed initialisation or to otherwise en-

14

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 15: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

(a) Semi-trailer with markers (b) Semi-trailer no markers (c) Drawbar

Figure 13 Full-trailer feature maps

sure sufficient motion between keyframes in an automatic fashion Adding a lateral offsetbetween the camera and hitch axis would help reduce the angle required for initialisationby increasing the translation component of the relative motion (T)

6 Performance comparison

A summary of the performance of the system is given in Table 1 together with theavailable performance information of comparable systems discussed in Section 11 Theencoder and infrared systems of [19] have not been included as they require trailer mod-ifications Information on RMS errors was not available from the published literaturebut has been included for the PTAM system in this case the average RMS error is givenover the range of manoeuvres and runs conducted The table has been separated intosystems which have been assessed only in a simulation environment and those whichhave undergone full-scale field tests

The PTAM systems performs well in simulation compared to other simulation-testedsystems The marker tracking systems perform better but rely on specially-located mark-ers placed on the trailer The template matching system also performs better but islimited to trailers with a planar front and is restrained to a maximum articulation an-gle In comparison the PTAM system was tested up to 84 Field test results providea more accurate view of expected in-field performance In field tests the PTAM systemexhibits the lowest maximum errors with a significantly smaller variation in performancecompared to some The PTAM system was also tested to higher articulation angles thanthe others and is theoretically unconstrained in this respect Also the PTAM systemhas been demonstrated to function with trailers with non-planar fronts (the state ob-server and stereo vision methods should theoretically manage this too) and has beendemonstrated to function effectively for a bi-articulated trailer (which none of the othersystems have) Overall RMS errors are shown to be in the region of 1 Provided themaximum errors can be reduced through filtering or further investigation into the errorsensitivities this indicates that the PTAM system has the potential for incorporationinto several articulated vehicle control systems in future

7 Error sensitivity

A sensitivity between measurement error and articulation angle was observed in all testssuggesting some shared error mechanism However in the tractor semi-trailer tests (Sec-tion 4) increasing Γ led to increasing positive errors (Γ is over-estimated) while inSections 3 and 5 increasing Γ led to increasing negative errors (Γ is under-estimated)This is demonstrated in a comparison of maximum error and maximum articulation an-

15

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 16: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Table 1 Comparison of articulation angle estimation performance

Method Validation Γmax () εmax () εRMS ()[16] State observer Simulation 90 8 -[17] State observer Simulation 3ndash20 03ndash10 -[21] Homography decompositiondagger Simulation 17ndash90 32ndash84 -[22] Marker trackingdagger Simulation 50 05 -[23] Marker trackingdagger Simulation 15ndash30 06 -[24] Template-matchingdagger Simulation 50 07 -

PTAM (semi-trailer)dagger Simulation 50ndash84 11ndash16 07[18] State observer Field tests 48 54 -[20] Template-matchingdagger Field tests 20ndash55 55ndash76 -[21] Stereo visiondagger Field tests 17ndash52 33ndash18 -[24] Template-matchingdagger Field tests 37 23ndash68 -

PTAM (semi-trailer)dagger Field tests 23ndash37 12ndash26 07PTAM (semi-trailer non-planar)dagger Field tests 28-50 17ndash38 12PTAM (Nordic trailer)dagger Field tests 54ndash61 35ndash51 15PTAM (Nordic drawbar)dagger Field tests 30ndash40 25-46 10dagger Computer vision method

Figure 14 Maximum errors vs max articulation angle all tests

gles shown in Figure 14 The results for all runs of all manoeuvres (both simulationand experiment) are included and a linear least squares fit has been added for each setof tests (A zero-intercept has been forced consistent with the observed behaviour withcorrect zeroing)

It is clear that the sensitivity is unique for each configuration of the camera-trailersystem The drawbar and semi-trailer measurements in the Nordic tests display a similarbehaviour and these share the same camera setup and environment It is also observedthat the theoretically ideal simulation tests also exhibit this sensitivity but to a smallerextent In this case the camera is theoretically perfectly aligned and calibrated withno distortion and the system has no pitch or roll degrees of freedom Therefore it isconcluded that an underlying systemic sensitivity exists which is shared by all tests andthat an additional environmental and physical sensitivity exists in the field tests

The underlying cause of these errors could be one or a combination of several factors

16

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 17: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

bull Inaccuracies in the initialised stereo map This would lead to increasing errors withincreasing Γ as the scene rotates further away from the initial keyframes The extent ofthis error could depend on the distance between keyframes or the number and verticaldistribution of features in the scene

bull The relatively simplistic camera model used Only radial distortion is assumed andonly one distortion coefficient is used to model this In reality lenses often give rise tolsquobarrelrsquo distortion which is a function of both radial and tangential distance requiringfour or more coefficients We would expect that inaccuracies in the distortion modelwould yield higher errors in the periphery of the field-of-view which is synonymouswith larger Γ

bull Inaccuracies in the zeroing process If zeroing is carried out at non-zero Γ a measure-ment bias would result If a non-zero pitch or roll angle exists during zeroing thiswould give rise to an error as a function of Γ

bull Camera positioning Inaccuracies in the camera mounting could yield unexpected mea-surement errors likely a function of Γ if there is an inherent pitch or roll offset Correctzeroing should take care of a yaw angle offset in the camera mounting

8 Conclusion

A concept for articulation angle estimation for articulated HGVs has been presentedutilising a rear-facing camera and image processing based largely on the PTAM algo-rithm of Klein and Murray [10] The system is entirely tractor-based non-contact doesnot require knowledge of or modification to the trailer units and runs in real-time at 20fps (20 Hz) The system has been validated in full-scale vehicle trials on both a tractorsemi-trailer combination (single articulation) and a Nordic combination (two articula-tions) and shown to yield good performance relative to the published literature Thesystem is theoretically expandable to other vehicle combinations with two or more ar-ticulation points this will require optimisation of the camera location and field of viewsegmentation Repositioning of the camera or the use of more than one camera may berequired in some instances especially in combinations with two or more trailers wherethe lead trailer is a semi-trailer such as a B-double (see Figure 1 and [32])

This concept presents a solution to the task of tractor-based non-contact articulationangle sensing for articulated vehicles which can assist in the commercialisation of vehiclecontrol systems which ultimately supports the uptake to long combination vehicles forthe benefit of economies and the environment It is expected that performance of thesystem can be improved in future work through optimisation and automation of thestereo initialisation and zeroing processes and through a more detailed investigationinto sensitivity to pitch and roll motion

Acknowledgements

The authors would like to thank Volvo Group Trucks Technology (GTT) for their supportwith field testing at Hallered especially Johan Eklov and Gustav Neander Thanks arealso extended to the CUED technical staff and colleagues in the Cambridge VehicleDynamics Consortium for their assistance with vehicle instrumentation and field testing(Will Midgley Matthew Miao Ayelet Ashkenazi Graeme Morrison Amy Rimmer LeonHenderson Yanbo Jia Richard Roebuck)

17

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 18: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

Funding

This work was funded by the Cambridge Commonwealth European and InternationalTrust (CCEIT) the Council for Scientific and Industrial Research (CSIR South Africa)and the Cambridge Vehicle Dynamics Consortium (CVDC) At the time of writing theConsortium consisted of the University of Cambridge with the following partners from theheavy vehicle industry Anthony Best Dynamics Camcon Denby Transport FirestoneIndustrial Products Goodyear Haldex MIRA SDC Trailers Tinsley Bridge TridecVolvo Trucks and Wincanton

Disclosure statement

No potential conflict of interest was reported by the authors

References

[1] Feige I Freight transport and economic growth ndash two inseparable twins In Transp trade econgrowth - coupled or decoupled Berlin Heidelberg Springer 2007 p 9ndash73 Available from httpsdoiorg101007978-3-540-68299-8_2

[2] Nordengen P Berman R de Saxe C Deiss J An overview of the performance-based standardspilot project in South Africa In Proceedings of the 15th International Symposium on Heavy Ve-hicle Transport Technology (HVTT15) Oct 2ndash5 2018 Rotterdam International Forum for RoadTransport Technology (IFRTT) 2018

[3] NHVR Performance Based Standards -An introduction for road managers 2019 Available fromhttpswwwnhvrgovaufiles201810-0924-pbs-a-guide-for-road-managerspdf

[4] Cheng C Odhams A Roebuck RL Cebon D Feedback control of semi-trailer steering at low speedsSubmitt to Proc Inst Mech Eng Part D J Automob Eng 2011

[5] Jujnovich BA Cebon D Path-Following Steering Control for Articulated Vehicles J Dyn Syst MeasControl 2013135(3)031006 Available from httpsdoiorg10111514023396

[6] Roebuck RL Odhams A Tagesson K Cheng C Cebon D Implementation of trailer steering controlon a multi-unit vehicle at high speeds J Dyn Syst Meas Control 2013 dec136(2)021016ndash021016ndash14Available from httpsdoiorg10111514025815

[7] Rimmer AJ Cebon D Implementation of reversing control on a doubly articulated vehicle J DynSyst Meas Control 2017 apr139(6)061011ndash061011ndash9 Available from httpsdoiorg10111514035456

[8] Morrison G Cebon D Combined emergency braking and turning of articulated heavy vehicles VehSyst Dyn 2017 may55(5)725ndash749 Available from httpsdoiorg101080004231142016

1278077[9] Chen LK Shieh YA Jackknife prevention for articulated vehicles using model reference adaptive

control Proc Inst Mech Eng Part D J Automob Eng 2011 jan225(1)28ndash42 Available from httpsdoiorg10124309544070JAUTO1513

[10] Klein G Murray D Parallel tracking and mapping for small AR workspaces 6th IEEE ACM IntSymp Mix Augment Real 2007 novAvailable from httpieeexploreieeeorglpdocsepic03wrapperhtmarnumber=4538852

[11] Aurell J Wadman T Vehicle combinations based on the modular concept Nordic Road Asso-ciation 2007 Report no Available from httpwwwnvfnordenorglisalibgetfileaspx

itemid=260[12] VSE Product information for ETS trailer 2009 Available from httpwwwv-s-ecomuploads

documentsleaflet-ets-trailer-enpdf[13] Cheng C Enhancing safety of actively-steered articulated vehicles Phd thesis University of Cam-

bridge 2009[14] Jujnovich BA Active steering of articulated vehicles Phd thesis University of Cambridge 2005[15] Product overview mechanical and hydraulic steering systems and axle suspensions Available

18

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 19: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

from httpjicjost-worldcomstaticuploadpdfFLY32004_FLY_TRIDEC_Uebersicht_

Image_160824_EN_SCREENpdf[16] Bouteldja M Koita A Dolcemascolo V Cadiou JC Prediction and detection of jackknifing problems

for tractor semi-trailer In IEEE Veh Power Propuls Conf sep Windsor IEEE 2006 p 1ndash6Available from httpsdoiorg101109VPPC2006364272

[17] Chu L Fang Y Shang M Guo J Zhou F Estimation of articulation angle for tractor semi-trailerbased on state observer In Int Conf Meas Technol Mechatronics Autom mar Vol 2 ChangshaCity IEEE 2010 p 158ndash163 Available from httpsdoiorg101109ICMTMA2010342

[18] Ehlgen T Pajdla T Ammon D Eliminating blind spots for assisted driving IEEE Trans IntellTransp Syst 20089(4)657ndash665 Available from httpsdoiorg101109TITS20082006815

[19] Schikora J Berg U Zobel D Beruhrungslose Winkelbestimmung zwischen Zugfahrzeug undAnhanger In Halang W Holleczek P editors Aktuelle anwendungen tech und wirtschaft In-formatik aktuell Springer Berlin Heidelberg 2009 p 11ndash20 Available from httpsdoiorg101007978-3-540-85324-4_2

[20] Caup L Salmen J Muharemovic I Houben S Video-based trailer detection and articulation esti-mation In IEEE Intell Veh Symp jun Gold Coast IEEE 2013 p 1179ndash1184 Available fromhttpsdoiorg101109IVS20136629626

[21] Harris MP Application of computer vision systems to heavy goods vehicles visual sensing of artic-ulation angle Mphil thesis University of Cambridge 2013

[22] Fuchs C Eggert S Knopp B Zobel D Pose detection in truck and trailer combinations for advanceddriver assistance systems In Proc IEEE Intell Veh Symp jun Dearborn MI IEEE 2014 p1175ndash1180 Available from httpsdoiorg101109IVS20146856547

[23] Fuchs C Neuhaus F Paulus D Advanced 3-D trailer pose estimation for articulated vehicles InIEEE Intell Veh Symp Seoul IEEE 2015 p 211ndash216 Available from httpsdoiorg101109IVS20157225688

[24] de Saxe CC Cebon D Measurement of articulation angle by image template matching Proceed-ings of the Institution of Mechanical Engineers Part D Journal of Automobile Engineering 2019Available from httpsdoiorg1011770954407019833819

[25] Montemerlo M Thrun S Koller D Wegbreit B FastSLAM A factored solution to the simultaneouslocalization and mapping problem In Proc 8th Natl Conf Artif Intell Conf Innov Appl ArtifIntell Vol 68 Edmonton Alberta AAAI 2002 p 593ndash598

[26] Montemerlo M Thrun S Roller D Wegbreit B FastSLAM 20 An improved particle filteringalgorithm for simultaneous localization and mapping that provably converges Proc Sixt Int Jt ConfArtif Intell 20031151ndash1156Available from httpsdoiorg101109IROS2006282528

[27] Grisetti G Kummerle R Stachniss C Burgard W A tutorial on Graph-Based SLAM IEEE IntellTransp Syst Mag 20102(4)31ndash43 Available from httpsdoiorg101109MITS2010939925

[28] Angeli A Doncieux S Meyer JA Filliat D Visual topological SLAM and global localization In2009 IEEE Int Conf Robot Autom may IEEE 2009 p 4300ndash4305 Available from https

doiorg101109ROBOT20095152501[29] Engel J Sch T Cremers D LSD-SLAM Large-Scale Direct monocular SLAM In Fleet D Pajdla T

Schiele B Tuytelaars T editors Proc 13th Eur Conf Comput Vis Part 2 Zurich Springer 2014p 834ndash849 Available from httpsdoiorg101007978-3-319-10605-2_54

[30] Mur-Artal R Montiel JMM Tardos JD ORB-SLAM A versatile and accurate monocular SLAMsystem IEEE Trans Robot 2015 oct31(5)1147ndash1163 Available from httpsdoiorg101109TRO20152463671

[31] Mur-Artal R Tardos JD ORB-SLAM2 an open-source SLAM system for monocular stereo andRGB-D cameras IEEE Transactions on Robotics 201733(5) Available from httpsdoiorg101109TRO20172705103

[32] Cebon D de Saxe CC WO2019202317 - Method and system of articulation angle measure-ment 2019 apr Available from httpspatentscopewipointsearchendetailjsfdocId=WO2019202317

[33] Stewenius H Engels C Nister D Recent developments on direct relative orientation ISPRS JPhotogramm Remote Sens 2006 jun60(4)284ndash294 Available from httpsdoiorg101016jisprsjprs200603005

[34] Rosten E Porter R Drummond T Faster and better a machine learning approach to cornerdetection IEEE Trans Pattern Anal Mach Intell 2008 jan32(1)105ndash19 Available from https

doiorg101109TPAMI2008275[35] Bridgewater AW Analysis of second and third order steady-state tracking filters In Rabinowitz SJ

editor AGARD Conf Proc no 252 Strateg Autom Track Initiat jun Monterey CA AGARD

19

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20

Page 20: Camera-based articulation angle sensing for Heavy Goods ...christopherdesaxe.com/publications/2020_ptam_unpublished.pdf · Camera-based articulation angle sensing for Heavy Goods

August 17 2020 Vehicle System Dynamics my˙ptam˙paper˙preprint˙test˙dc˙corrections˙vsd˙format˙reducefigs

1979[36] Hartley R Zisserman A Multiple View Geometry 2nd ed Cambridge UK Cambridge University

Press 2003[37] Cipolla R Computer vision and robotics projection (4F12 lecture notes Cambridge University

Engineering Department) 2013[38] Devernay F Faugeras O Straight lines have to be straight Mach Vis Appl 200113(1)14ndash24

Available from httpsdoiorg101007PL00013269[39] Triggs B McLauchlan PF Hartley RI Fitzgibbon AW Bundle Adjustment mdash A Modern Synthesis

In Vis algorithms theory pract Vol 1883 Springer Berlin Heidelberg 2000 p 298ndash372 Availablefrom httpsdoiorg1010073-540-44480-7_21

[40] Parallel Tracking And Mapping for small AR workspaces Licence and source code download page2008 Available from httpwwwrobotsoxacuk~gkPTAMdownloadhtml

[41] La Valle SM Geometric representations and transformations In Plan algorithms 1st ed Chapter 3Cambridge University Press 2006 p 81ndash126 Available from httpplanningcsuiucedu

20