Thesis JLF

Kinematic Analysis Of A Two-Body Articulated
Robotic Vehicle
Jesse Lee Farmer
Thesis submitted to the faculty of the Virginia Polytechnic Institute and State
University in partial fulfillment of the requirements for the degree of
Master of Science
in
Mechanical Engineering
Dr. Charles F. Reinholtz, Chairman

Alumni Distinguished Professor, Virginia Tech
Chair, Mechanical Engineering, Embry Riddle Aeronautical University
Dr. Alfred L. Wicks, Co-Chairman

Associate Professor of Mechanical Engineering, Virginia Tech
Dr. Dennis W. Hong

Assistant Professor of Mechanical Engineering, Virginia Tech
February 8, 2008
Blacksburg, Virginia
Keywords: kinematic model, four-bar linkage, vehicle off-tracking

Kinematic Analysis Of A Two-Body Articulated Robotic
Vehicle
Jesse Lee Farmer
ABSTRACT
The kinematic analysis of an articulated twin-body, four-wheel, robotic vehicle is pre-

sented. Polaris, a research platform and contending robotic vehicle in the Intelligent
Ground Vehicle Competition (IGVC) at Virginia Tech, was redesigned in 2006 to improve
the mobility of the vehicle by incorporating an innovative four-bar linkage that connects
the two bodies. The new linkage design minimizes vehicle off-tracking by allowing the
rear wheels to closely track the path of the front wheels. This thesis will outline the theo-
retical kinematic model of the four-bar linkage as applied to a twin-bodied, differentially
driven vehicle. The kinematic model is validated through computer simulation as well as
experimentation on a fully operational robotic vehicle. The kinematic model presented
here outlines the foundations for an autonomous, four-wheel drive, multi-body control
system and opens avenues for dynamically controlling the tracking of the vehicles rear
body with an actuated linkage configuration.
Acknowledgments
This thesis, as well as my graduate career, would not have been possible without the
help from the following individuals. First of all, I would like to thank God for giving
me strength and guidance throughout my life. I would also like to thank my family and
friends for their interest, support, and pride throughout my academic carrier. Without this,
I would not be where I am today.
I would like to give thanks to Dr. Charles Reinholtz, Dr. Alfred Wicks, and Dr. Dennis
Hong. Not only have you been my mentors throughout my graduate career, you have been
great friends. The camaraderie you have with your students both during and after school
is irreplaceable. Thank you for taking me under you wing and letting me be a part of this
experience.
Next, I would like to give credit to the 2007 Virginia Tech IGVC team for all of their
hard work. Thank you all for listening to me (at times) and allowing me to be your adviser.
This experience of working with these young engineers has fueled my interest in teaching.
Also, I would like to express my gratitude to the other graduate advisers, Peter King
and Jon Weekly. Peter, thank you for all your help and long hours spent scratching our
heads on the kinematic simulation. Without your help I believe I would still be sitting
in the WARE lab with no hair and no solution. Jon, thank you for all your help with
LabVIEW and data acquisition, not only on this project, but with all the others as well.
I need to acknowledge TORC Technologies LLC. and its employees, not only for their
guidance and expertise on the 2007 IGVC team, but for their work and consultation on
the 2007 DARPA Urban Challenge.
Finally, I would like to give thanks to my mentor and friends from Bluefield State
College, Dr. Bob Riggins and his wife Debbie Riggins. Thank you for sparking my
interest in robotics as well as providing me with the knowledge and desire to pursue a
higher education.
iii
Contents
Chapter 1: Introduction 1
1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Thesis Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
Chapter 2: Literature Review 5

2.1 Vehicle Off-Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2 Patent Designs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
Chapter 3: Base Vehicle 8

3.1 Intelligent Ground Vehicle Competition . . . . . . . . . . . . . . . . . . 8
3.2 Vehicle Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.2.1 Chassis and Drive Train . . . . . . . . . . . . . . . . . . . . . . 10
3.2.2 Hardware and Electrical Components . . . . . . . . . . . . . . . 11
3.2.3 Sensors and Computing . . . . . . . . . . . . . . . . . . . . . . 12
3.3 Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
Chapter 4: Kinematic Model 15

4.1 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
4.2 Front Body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
4.3 Four-Bar Linkage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.3.1 Position Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.3.2 Velocity Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 23
Chapter 5: Software Development 28

5.1 MATLAB Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
5.2 Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
5.2.1 Kinematic Verification . . . . . . . . . . . . . . . . . . . . . . . 33
5.2.2 Linkage Verification . . . . . . . . . . . . . . . . . . . . . . . . 36
iv
Chapter 6: Conclusion 40
6.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
6.2.1 Linkage Design . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
6.2.2 Four-Wheel Drive Vehicle . . . . . . . . . . . . . . . . . . . . . 42
6.2.3 Dynamically Controlled Off-Tracking . . . . . . . . . . . . . . . 42
6.2.4 Second Degree Of Freedom . . . . . . . . . . . . . . . . . . . . 42
References 44
Appendix A: Analytical Position Analysis for Four-Bar Linkage 46
Appendix B: Analytical Solution Of Crossing Point P 49
Appendix C: MATLAB Simulation Code 51

C.1 Simulation Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
C.2 Four-Bar Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
C.3 Four-Bar Plot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
C.4 Simulation Iteration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
v
List of Figures
1.1 Setup for acoustic array data collection. . . . . . . . . . . . . . . . . . . 2

1.2 Proof-of-concept model for the four-bar linkage design. . . . . . . . . . . 4
2.1 Illustration of low-speed truck off-tracking. . . . . . . . . . . . . . . . . 6
3.1 Polaris vehicle demonstrating its two degree-of-freedom mobility and sup-
port bar structure on a 10 degree incline. . . . . . . . . . . . . . . . . . . 11
3.2 Polariss system architecture for 2007 IGVC events. . . . . . . . . . . . . 13
4.1 Illustration of the vehicles coordinate frames within the global frame. . . 16
4.2 Illustration of the constraints on the front body. . . . . . . . . . . . . . . 18
4.3 Illustration of the four-bar linkage frame in the front body frame. . . . . . 22
4.4 Illustration of the instantaneous center of point (P) with respect to the
front body. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
4.5 Illustration of the instantaneous center of the rear body with respect to
point P. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
5.1 Software architecture for the kinematic simulation. . . . . . . . . . . . . 29

5.2 Plot of the linkage configuration. . . . . . . . . . . . . . . . . . . . . . . 30
5.3 Plot of the vehicle position in the global coordinate frame. . . . . . . . . 31
5.4 Plot of the trajectory taken by the vehicles wheels in the global coordinate
frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
5.5 Plot of vehicle parameters using Baitys simulation (a) and plot of vehicle
parameters using this simulation (b). . . . . . . . . . . . . . . . . . . . . 33
5.6 Plot of vehicle position from Baitys simulation (a) wheel trajectory using
Baitys simulation (b). . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5.7 Plot of vehicle position from the simulation from this paper (a) wheel
trajectory using the simulation developed in this thesis (b). . . . . . . . . 35
5.8 Plot of wheel velocities versus time from both simulations. . . . . . . . . 35
vi
5.9 Scatter plot of the residuals from the wheel velocities. . . . . . . . . . . . 36
5.10 Plot of vehicle parameters using Baitys simulation (a) and plot of vehicle
parameters using this simulation (b). . . . . . . . . . . . . . . . . . . . . 37
5.11 Plot of vehicle position from Baitys simulation (a) wheel trajectory using
Baitys simulation (b). . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.12 Plot of vehicle position from the simulation from this paper (a) wheel
trajectory using the simulation developed in this paper (b). . . . . . . . . 38
5.13 Plot of wheel velocities verses time from both simulations. . . . . . . . . 39
5.14 Scatter plot of the residuals from the wheel velocities. . . . . . . . . . . . 39
vii
List of Tables
3.1 Summary of Sensors Used In The 2007 IGVC Events. . . . . . . . . . . . 14
4.1 Variables Used For Front Body Parameters. . . . . . . . . . . . . . . . . 19
5.1 Simulation Initial Conditions and Parameters. . . . . . . . . . . . . . . . 29
viii
Chapter 1
Introduction
The work presented in this paper was proposed to improve the mobility of an existing
vehicle platform, Polaris, for competing in the 2007 Intelligent Ground Vehicle Compe-
tition (IGVC). Likewise, this research proposes a concept for dynamically controlling the
path of the rear section of a two-body vehicle by changing the linkage that connects the
front to the rear sections.
1.1 Motivation
The Polaris vehicle was first developed in 2004-2005 academic year by a group of under-
graduate students as members of the Autonomous Vehicle Team (AVT) at Virginia Tech.
The Polaris platform is a four wheeled, articulated twin-body, robotic vehicle used to
compete in the annual IGVC events, as well as a robotic research platform at Virginia
Tech. With its debut in the 2005 IGVC at Traverse City, Michigan, Polaris took second in
1
the Design Competition and third place in both the Autonomous Challenge and the Nav-
igation Challenge. Even though the vehicle performed well at the 2005 IGVC, its large
two-body design seemed cumbersome and hindered the maneuverability of the platform
in tight passageways. The most obvious problem that occurred at the 2005 IGVC was
when the front body made a tight turn around an obstacle, the rear body would clip the
object, usually to the inside path of the front body. This behavior is known as vehicle
off-tracking. This phenomenon is commonly seen in tractor-trailer vehicles that use a
kingpin hitch. When taking a sharp turn at low speeds, the rear axles of the vehicle will
cut to the inside of the path taken by the front axle [7]. For this reason, Polaris was not
entered in the 2006 IGVC and continued its role at Virginia Tech as a research platform
for work in acoustic detection with an autonomous ground vehicle (Figure 1.1) [18], as
well as an autonomous broccoli inspection robot [17].
Figure 1.1: Setup for acoustic array data collection (2006).
Due to the rugged construction and reliability of the vehicle, AVT made the decision to
2
reuse the Polaris platform for re-entry into the 2007 IGVC. Reusing the existing platform
eliminated the time spent on the design and fabrication of a new platform, allowing the
team to focus their efforts on the sensing and programing aspects of the competition.
However, in order to effectively use the Polaris platform, the issue with the vehicles
off-tracking needed to be addressed.
1.2 Thesis Overview
After reviewing related literature on vehicle off-tracking and work on reducing vehicle
off-tracking, a four-bar linkage-based joint was proposed as a means of controlling the
relative motion of the two bodies. Once the concept was developed, an early goal of this
research project was to construct a simple model for testing and demonstrating proof-of-
concept of initial designs. This involved creating a mock-up of the Polaris II platform,
but leaving the linkage connecting the front and rear sections adjustable to allow changes
in the configuration. Figure 1.2 shows the model constructed by members of the 2007
AVT and shows a four-bar linkage configuration that will be the focus of this research.
After the proof-of-concept of the four-bar linkage was confirmed, a computer simu-
lation was developed to predict the path of the front and rear wheels. In this simulated
environment, the linkage configurations could easily be changed and tested in many dif-
ferent scenarios to determine which configuration would be optimal for construction on
the actual Polaris II vehicle. Using the dimensions of the vehicle along with initial front
wheel speeds and body pose as input parameters, the simulation is able to calculate the
vehicles position and orientation through kinematic analysis.
After rigorous testing and validation in simulation, a linkage design was chosen and
then implemented on the Polaris II platform. However, there were many design con-
straints that limited the application of the original proof-of-concept design. For example,
3
Figure 1.2: Proof-of-concept model for the four-bar linkage design.
a desired vehicle feature was to allow independent roll between the two bodies. This al-
lows all four wheels to remain in contact with the ground on uneven terrain without the
use of suspension. To incorporate this into the design of the mechanical linkage, a revo-
lute joint was added between the rear body and the linkage frame to allow roll between
the two bodies. This, along with additional design constraints, had to be considered in the
application of the four-bar linkage into the preexisting Polaris platform and is discussed
in more detail in Chapter 3.
To measure the performance of the simulation and kinematic model, the outputs from
this simulation are compared to a similar kinematic model created for the original Polaris
platform.
Finally, recommendations for future work are presented.
4
Chapter 2
Literature Review
2.1 Vehicle Off-Tracking
Vehicle off-tracking is a phenomenon largely studied in the design of highway exit ramps,
intersections, and other highway components. Careful consideration is given to areas of
a road where a vehicles off-tracked path could leave the pavement and onto the road-
way shoulder or encroach into other lanes of traffic. Two types of vehicle off-tracking
exists, recognized as low-speed off-tracking and high-speed off-tracking [8]. Due to the
limitations on speed (maximum of 5mph) in the IGVC events, this research will focus on
low-speed off-tracking.
Low-speed off-tracking can be observed when a vehicle traveling at low speeds ne-
gotiates a curve. The vehicles rear wheels encounter a sliding force due to the direction
of travel taken by the front wheels. This sliding force causes the difference between the
front and rear wheel paths, usually cutting the path of the rear wheels to the inside of a
5
curved road. The magnitude of the radial difference between the path of the front axles
centerline and the centerline of the rear axle is known as off-tracking amount (Figure
2.1) [7].
Figure 2.1: Illustration of low-speed truck off-tracking. [7]
2.2 Patent Designs
The goal of this section is to give the reader a brief overview for a variety of patent
linkage designs for trailered vehicles. The patents referenced in this section are from a
small sample of related designs for trailer linkages and does not reflect the vast number
of designs currently patented for trailer towing systems.
6
There are many patent designs currently developed to improved steering and stability
of an assortment of trailer apparatuses, including long trains of trailers for cargo and
material handling. Many of these patents add complexity to a trailer linkage design by
incorporating an intermediate axle and/or motorized controls to help steer the trailer being
towed [20], [5], [6], [2]. This added complexity has the potential to increase the cost and
probability of failure to the vehicle. Other patents incorporate a floating pivot into the
trailer linkage design to enhance the turning characteristics of a trailer [16], [10], [9]. The
drawback to this type of design is that the turning angle of the trailer is limited to the
amount of pivot in the linkage which is dependent on the length of the slot that guides the
pivotal point.
From the variety of linkage designs studied, the inventions of Fredric R. Jennings [12]
and Leon Meadows [15] are the most intriguing. Jennings invention is for a true tracking
trailer system used for carrying cargo with a train of trailers. The patent characterizes the
trailered vehicle used and the linkage connecting the trailers. This linkage design uses a
cross-linked mechanism similar to the one depicted in Figure 1.2. Similarly, the invention
by Meadows uses this same linkage configuration. However, Meadowss patent is for an
aparatus used specifically for backing a towed trailer. These patent designs, along with
the design in ( [9], [10]), indicate the use of a cross-link mechanism for improving the
tracking characteristics of a trailer-type vehicle.
7
Chapter 3
Base Vehicle
3.1 Intelligent Ground Vehicle Competition
To better understand the application of the Polaris II vehicle, a brief explanation will be
given on the IGVC event.
Initially funded by Association for Unmanned Vehicles Systems International (AU-
VSI) in 1993, the IGVC has grown to include many sponsors from the US military and
industry leaders. This multidisciplinary, student-oriented competition is designed to ad-
vance the students knowledge in mobile robotics. The IGVC brings both graduate and
undergraduate students together to contribute toward a team effort of managing, design-
ing, building, and entering their robots into competition against colleges and universities
from around the world. The competition is usually divided into three major events: De-
sign Competition, Autonomous Competition, and Navigation Challenge. In the Design
Competition, teams are judged on the overall design of their vehicle through formal writ-
8
ten reports and an oral presentation. The Autonomous Competition requires the robot to
traverse an outdoor obstacle course, within a predefined time limit, while staying within
the courses lane boundaries and avoiding static obstacles. The Navigation Challenge tests
a robots ability to autonomously navigate using Global Positioning System (GPS) way-
points and landmarks to accomplish a number of target destinations. Often, there will be
a fourth competition, usually involving leading-edge technology in autonomous mobile
robots. For the past two years the fourth competition has involved implementing Joint
Architecture for Unmanned Vehicles (JAUS) messages into the robots communication
protocol. Participation in this event is optional and includes a written/oral presentation
along with a practical demonstration of the JAUS messaging. Further information on the
IGVC events can be found on their website [11].
Since entering in its first IGVC in 1996, the Autonomous Vehicle Team (AVT) at Vir-
ginia Tech has annually placed among the top two contenders in the Autonomous,Design,
and Navigation challenges. In fact, Virginia Techs AVTs has won first place in the De-
sign Competition nine of the twelve competitions it has entered. It has also finished first
in the Autonomous Challenge for the last five consecutive years from 2003-2007 and in
the Navigation Challenge for the last three consecutive years from 2005-2007 [11]. This
outstanding record has made the Virginia Tech AVT the leading contender among the
competitors at the IGVC events, as well as with future AVT groups.
3.2 Vehicle Overview
The Polaris platform was originally constructed in 2005, but went through a redesign in
2007. Even though the original design of Polaris produced a rugged and reliable vehicle
that performed well in the 2005 IGVC events, the redesign in 2006 incorporated many
new features that optimized the original design. This section will first cover the vehicles
9
chassis and drive train. Next, the vehicles hardware and electrical components will be
discussed. Lastly, the sensors and computing equipment used on the Polaris II platform
will be mentioned.
3.2.1 Chassis and Drive Train
The vehicles frame is constructed from one inch square aluminum tubing, with 0.125
inch wall thickness. The overall dimensions of the vehicle are 56 inches long, 34 inches
wide, and 68 inches tall [1]. The overall height of the vehicle is capable of being changed
due to a collapsible mask that allows for ease of transportation.
The linkage connecting the front and rear bodies was designed with a two degree-
of-freedom motion to allow both yaw and roll of the front platform (Figure 3.1). The
added ability of body roll between the two sections replaces the need for a suspension
system on the vehicle. This body roll motion allows all four wheels to stay connected
to the ground when traversing uneven terrain. To reduce pitch between the two bodies
on uneven terrain and during initial starting torque, a support-bar made of plastic was
placed between the linkage links and the frame of the body (Figure 3.1). This support-
bar strengthened the stability of the linkages by increasing the surface area connecting the
linkage to the body, while the plastic material reduced friction between the mating parts.
Likewise, the linkage connecting the bottom front and rear sections was configured in a
way to minimize interference between the vehicle and the ground.
The total unloaded weight of the vehicle is 210 pounds, with 60 percent distributed
on the front body and 40 percent on the rear body [1]. This configuration positions the
larger weight distribution over the drive wheels to providing better traction for vehicle
locomotion.
The vehicles drive train incorporates two QuickSilver I-Grade 34HC-2 brushless DC
motors that allow the vehicle to perform a zero-radius turn by differentially controlling the
10
Figure 3.1: Polaris vehicle demonstrating its two degree-of-freedom mobility and support
bar structure on a 10 degree incline.
right and left wheel velocities. These motors are controlled by two QuickSilver I-Grade
N3 SilverNugget controllers. The motors have a maximum power of .76 horsepower and
maximum stall torque of 6.78 foot-pounds [1].
3.2.2 Hardware and Electrical Components
The Polaris II vehicle is powered by two 12V lead acid batteries connected in series
to supply 24V power to the power distribution board. This configuration will provide
four hours of runtime to the vehicle. The power distribution board designed by the 2007
AVT provides five regulated 12V circuits and four regulated 24V circuits. Each circuit is
independently fused which provides protection to all of the components that are powered
on the vehicle.
11
3.2.3 Sensors and Computing
Polaris uses a number of sensors to allow it to navigate waypoints and avoid obstacles.
The sensor suite presented in this section was used to compete in the 2007 IGVC events,
but can be easily reconfigured to perform a variety of different tasks.
The two major components that give the robot a sense of where it is located in the
world are the GPS system and the digital compass. The GPS system used at the IGVC
event was the Novatel Propack LB+. This system uses Differential Global Positioning
System (DGPS) which uses a constellation of satellites orbiting the earth to triangulate a
position on the surface of the earth. The DGPS also uses correction measurements taken
by local ground base antennas to give a more accurate position estimate. The digital
compass used on Polaris II is the PNI TCM2-20 Digital Compass. This sensor uses
gyroscopes and accelerometers to determine the vehicles heading, relative to magnetic
north. The digital compass is primarily used to determine the vehicles initial heading and
is then compared to the heading measurements received from the DGPS system.
The sensors that are used for obstacle detection are the Laser Measurement System
(LMS) and the digital camera. The LMS sends out infrared light and measures the time-
of-flight of the reflected light to get an accurate position measurement. The LMS used on
Polaris II is the SICK LMS-291 and is primarily used to detect obstacles such as barrels,
cones, fences, and poles. The digital camera used on Polaris II during the 2007 IGVC
is the Unibrain Fire-I Board Camera. The cameras main use in the IGVC events is to
detect lane boundary lines and potholes, both represented by white field paint. The two
types of sensor data from the LMS and digital camera are mapped onto a Cartesian-style
occupancy grid, simplifying the obstacle avoidance algorithm [4].
The robots brain or central processing unit uses a Toshiba Portege M400 tablet
computer. The computer is responsible for acquiring data from the various sensors, pro-
cessing the data, and sending commands to control the motors. The Polaris II vehicle uses
12
National Instruments LabVIEW 8.2 for all of its data acquisition and computational pro-
graming. The vehicles motion control (heading and speed) is sent as serial commands via
USB to serial converters. These serial commands represent left and right motor speeds.
The system architecture of the Polaris vehicle used in the 2007 IGVC events is detailed in
Figure 3.2. Similarly, Table 3.1 describes the specifications of the sensors in more detail.
Figure 3.2: Polariss system architecture for 2007 IGVC events.
3.3 Performance
The Polaris vehicle has placed among the top three finishers in each of the 2005 IGVC
events. In 2005 Polaris won first place in the Design Competition, while placing second
and third in the Navigation and Autonomous Challenges, only to be beat by older con-
tenders from the Virgina Tech AVT [11]. In 2007 Polaris II placed third in the Design
Competition, but was unable to place in the Autonomous Competition due to a commu-
nications problem with its sensors. This communications issue also kept Polaris II out of
the Navigation Challenge for the 2007 year.
13
Table 3.1: Summary of Sensors Used In The 2007 IGVC Events.
14
Chapter 4
Kinematic Model
The kinematic model of the Polaris II vehicle is described in detail in this section along
with the mechanics that govern the vehicles movement. The methodology of the kine-
matic model is to develop a model of the vehicles motion as a function of time that will
predict the position and orientation of the vehicle due to front wheel velocities and initial
body pose. The kinematic model is conducted through a three-step process that involves
determining the position and velocity of the front body (1), then calculating the position
of the links in the four-bar linkage (2), followed by analyzing the position, orientation,
and wheel velocities of the rear body (3).
For this analysis and in simulation, the vehicles second degree-of-freedom, described
as the roll between the two bodies, will be neglected. The planar kinematic analysis of an
articulated, two-body vehicle is performed with the assumptions of no wheel slip and that
all four wheels remain in contact with the ground.
15
4.1 Coordinate Systems
The model is broken into three distinct members: front body, four-bar linkage, and rear
body. Each member is constructed on a two-dimensional, Cartesian based, coordinate
system know as the global frame (Figure 4.1). The coordinate system is drawn so that
the z-axis is pointed up, the y-axis is pointed forward and the x-axis is to the right. This
convention would allow an innate view of motion in the z direction for future work that
could include the two degree-of-freedom motion mentioned earlier. This is different from
the aerospace convention of defining the z-axis down, x-axis forward, and the y-axis to
the right [13].
Figure 4.1: Illustration of the vehicles coordinate frames within the global frame.
16
For a vehicle with known dimensional constraints on the body, the position of the
front body can be defined in the global frame (XG ,YG ) by the location of the center of axle
(CAF ) and the angle ( ) between the two frames. A rotation matrix (R ) is used to map
the motion of the front body in the global frame through a rotation about the z-axis of
(CAF ).
cos sin 0

R =
sin cos 0
(4.1)

0 0 1
The position and orientation of the rear body, relative to the front body, can be found
through locating the crossing point (P) of the links in the four-bar. This intermediate
coordinate system (CL1 ) is relative to the front and rear bodies, but will be discussed in
greater detail later in this section. The rear body is positioned relative to the front body
by the location of the center of axle (CAR ) and the angle().
4.2 Front Body
The forward kinematics of the front body is formulated by using the wheel speeds and
geometric constraints of the vehicle. A modification of the notation and conventions
used by Siegwart and Nourbakhsh is used throughout the analysis of the front body [19].
The motion of the front body can be represented in the global frame by a vector (CAF )
that includes its X and Y position as well as the angular rotation ( ) between the two
coordinate frames.
X

CAF =
Y
(4.2)

17
) that defines
Taking the derivative of the position vector produces a velocity vector (CA F
the motion of the front body within the global frame.

X

CA Y
= (4.3)

F

Given the rotational speeds of the front left and right wheels (LF , RF ), their respec-
tive radii (rLF , rRF ), and the distance (l) to the center axle (CAF ), the model can predict the
velocity of the front body (Figure 4.2). For this differentially driven vehicle, the two front
drive wheels are fixed so that a positive angular rotation produces a positive displacement
along the (YF ) axis. Using Siegwart and Nourbakhshs rolling and sliding constraints for
a fixed standard wheel ensures that all rotational motion exerted by the wheels produces
an accompanied translation motion of the front body.
Figure 4.2: Illustration of the constraints on the front body.
18
To simplify the analysis of the rolling and contact constraints, the assumptions are
made such that the wheels remain vertical to the plane of motion and have a single point
of contact to the ground plane. Likewise, the model will assume that there is no sliding
motion orthogonal to the rolling motion at the wheels single point of contact.
The variables for the rolling and sliding constraints (J1 , C) of the front body are shown
in Figure 4.2 with respect to the vehicles front body frame (XF ,YF ). The position of the
wheels is defined by the length (l) from the center of the axle (CAF ) and the angle (). The
angle of the wheel plane with respect to the vehicle chassis is expressed by ( ). The values
used for the variables ( and ) are constants determined by the vehicles construction
and are represented in Table 4.1. The variable (l) is a constant that is determined by the
width of the vehicle from center of axle to the wheel.
1 /2
2 /2
1
2 0
Table 4.1: Variables Used For Front Body Parameters.
An expression for the rolling constraints (Equation 4.4) is determined through the
construction of the vehicle and the constraints on the vehicles movement.

cos 1 + 1 sin 1 + 1 (l) cos 1
J1 = (4.4)
cos 2 + 2 sin 2 + 2 (l) cos 2
The rotational velocity of each wheel (J2 ) can be expressed as the wheels angular
rotation () multiplied by the radius (r) of each wheel.

rRF 0 RF
=
J2 () (4.5)
0 rLF LF
19
are
To ensure there is pure rolling at the contact point, the wheel speeds (J2 ())
constrained to produce motion along the direction of the wheel plane. The two terms
) are used to map the motion of the front wheels to the global frame.
(R , CA F
) = J2 ()
J1 (R )(CA (4.6)
F
Similarly, the sliding constraint (Equation 4.7) is used for the right and left wheels to
ensure that the sliding motion orthogonal to the wheel plane will be zero.

sin 1 + 1 cos 1 + 1 (l) sin 1
C= (4.7)
sin 2 + 2 cos 2 + 2 (l) sin 2
The sliding constraint is expressed in the global frame as
)=0
C(R )(CA (4.8)
F
Equations 4.6 and 4.8 are combined to form a single expression for the motion of the
front body in the global frame.

J1
J2 ()
=
R CA (4.9)
F
C 0
4.3 Four-Bar Linkage
Using the initial orientation of the front body and rear body, represented as the angles
( ) and () respectively, the position of the planar four-bar linkage connecting the two
bodies can then be determined. Using an iterative process of stepping through time (t)
and displacing the front body according to the wheel velocities of the front wheels, the
20
position of the four-bar linkage at each time step is evaluated using kinematic position
analysis. With the position of the linkage known, a velocity analysis can be conducted to
determine the velocity of the four-bar caused by the motion of the front body. Knowing
the position and velocity of the four-bar linkage, the transmission of motion from the front
body to the rear body can be resolved.
Up to this point the kinematic model has been developed to be vague enough to handle
any type of linkage configuration connecting the front and rear bodies. Through prelim-
inary experimentation with the simple model shown in Figure 1.2, the configuration of
having the links crossed proved to be ideal for allowing the rear body to mimic the path
of the front body. This configuration will be illustrated in this section, but the equations
presented here hold true for many four-bar configurations.
4.3.1 Position Analysis
To simplify the position analysis of the four-bar mechanism, it is separated from the front
body by a link offset (LOF ) and into its own coordinate system (CL1 ), where link1 is con-
sidered the ground plane (Figure 4.3). The link offset (LOF ) is a variable that expresses
the length from the center of the front axle (CAF ) to the center of link1 and is determined
by the construction of the vehicle.
A closed-form method is used to perform the position analysis of the four-bar linkage.
With this type of analysis the links in the mechanism form a closed loop, so the x and y
components of the links must equate to zero (Equation 4.10).
link1 + link2 e j2 + link3 e j3 + link4 e j4 = 0 (4.10)
The link lengths of link1 through link4 are constants that determine the configuration
of the mechanical linkage connecting the two bodies. The angle 3 , also known as , is
21
Figure 4.3: Illustration of the four-bar linkage frame in the front body frame.
a variable that indicates the orientation of the rear body. The initial value of this angle
is known by selecting the starting position of the vehicle. The angles 2 and 4 are the
unknowns that need to be found to close the loop. Appendix A gives the analytical solu-
tion for 2 in equation 4.10. The angle 4 is solved by plugging the solution for 2 back
into the loop closure equation 4.10. The arctan2 function, found in many computer
software languages, is used to solve for 4 to evaluate the angle in its correct quadrant for
a Cartesian coordinate system.
The crossing point (P) of link2 and link4 is used in the velocity analysis of the link-
age. point (P) can be found by using the point slope formula (Equation 4.11) where (m)
representing the slope of a line and (x, y) defining the coordinates of a line are the known
variables from the kinematic position analysis. The variables (x0 , y0 ) are the unknowns
22
that symbolize the coordinates of a point on the line. Appendix B gives the analytical
solution of point (P).
y y0 = m(x x0 ) (4.11)
4.3.2 Velocity Analysis
Since the front wheel velocities identified by the analysis of the front body are known,
the velocity of the rear wheels is desired to complete the kinematic analysis. In order
to determine the effects the linkage configuration has on the motion of the rear body,
the intermediate velocity of the linkage is required. Analyzing the affects the motion of
the front body has on the linkage, a sequential calculation can be made to determine the
effects the linkage has on the velocity of the rear body.
Velocity Of The Four-Bar
With the position of the links known, the linear velocity of the linkage can be found using
the instantaneous center of velocity method (instant center). By finding the instantaneous
pivot point of a body, known as the instant center, the velocity of the body can be described
as being in pure rotation about the pivot point for that instance in time. If the position of
two points on the body is known (l f w, r f w), then the location of the instant center (R f )
can be determined geometrically as the intersection of the lines which go through points
(l f w) and (r f w) and are perpendicular to (Vl f ) and (Vr f ) (Equation 4.12)(Figure 4.4). If
(V~l f > V~r f ), then (R f ) is positive. Likewise, if (V~l f < V~r f ), then (R f ) is negative.
l f (V~r f + V~l f )
Rf = (4.12)
(V~l f V~r f )
Knowing the position of the instant center (IC f ) and the velocity of a point on the
23
Figure 4.4: Illustration of the instantaneous center of point (P) with respect to the front
body.
body (Vl f or Vr f ), the angular velocity of the front body ( f ) can be determined.
V~ f
f = (4.13)
Rf
The distance (K f e jK f ) from point (P) to the instant center (IC f ) can be determined by
inspection (Equation 4.14)(Figure 4.4).
K f e jK f = R f + Pf e jP f + LO f (4.14)
The value of point (P) (V~P ) can be found by taking the cross product of the angular
velocity of the instant center of the front body (Equation 4.13) with the length of the line
24
from point (P) to the instant center (K f ).

~
Vp i K f cos K f 0

V~ j = K sin 0 (4.15)
p f Kf

V~p k 0 f
Note that when the angular velocity of the front body is infintesmally small (i.e. when
in pure translation), the instant center of rotation (IC f ) will be infinite. For this case, the
velocity of point (P) is simply the velocity of the front wheels (V~ f ) at the angle of P (P ).
V~p = V~ f e jP f (4.16)
Velocity Of The Rear Body
With the velocity of point (P) known (Equation 4.15), we can use a similar analysis of
instantaneous center of velocity for the rear body (Figure 4.5). By inspection, the velocity
of point (P) (V~P ) can be written in terms of the angular velocity of the rear body (Equation
4.17).
V~p = jr Kr e jKr (4.17)
A substitution for (Kr e jKr ) can be made to reduce the amount of unknown variables
in Equation 4.17 (Equation 4.18). The length from point (P) to the center axle of the rear
body (Pr e jP r ) is known from the position analysis of the four-bar and the length of the
offset from the center axle of the rear body to link3 (LOr ) (Figure 4.5).
Kr e jKr = Rr + Pr e jPr (4.18)
By expressing Equation 4.17 in terms of the cross product of the angular velocity (r )
25
Figure 4.5: Illustration of the instantaneous center of the rear body with respect to point
P.
with the substitution for (Kr ) (Equation 4.18), an expression for the unknown variables
(r and Rr ) can be determined (Equation 4.19).

~
Vp i Rr + Pr cos Pr 0

V~ j = Pr sin Pr 0
p

V~p k 0 r

~ r Pr sin Pr
Vp i

V~ j = (R + P cos ) (4.19)
p r r r Pr

V~p k 0
From Equation 4.19, an expression for the angular velocity of the instant center of the
26
rear body (r ) (Equation 4.20) and the length of (Rr ) (Equation 4.21) can be determined.
V~p i
r = (4.20)
Pr sin Pr
V~p j
Rr = + Pr cos Pr (4.21)
r
Now, using Equation 4.20 and Equation 4.21, the velocity of the left and right rear
wheels, denoted as (V~l r) and (V~r r) respectively, can be determined using the angular ve-
locity (r ) times the length from the instant center to the center axle of the rear body (Rr )
plus or minus the width of the rear wheel to center of the axle (lr ) (Equation 4.22). It must
be noted that in the case where the front body is in pure translation and the front and rear
bodies have the same pose (i.e. = ), then the velocity of the rear wheels will equal to
the velocity of the front wheels (V~l f = V~l r and V~r f = V~r r).
V~l r = r (Rr lr )
V~r r = r (Rr + lr ) (4.22)
27
Chapter 5
Software Development
Using the theoretical kinematic model described in Chapter 4 a simulation can be created
to analyze and visually inspect the outputs. Through an iterative process, the simulation
calculates the position and orientation of the vehicle due to the velocities of the front
wheels. A diagram of the software architecture for this simulation is illustrated in Figure
5.1.
5.1 MATLAB Code
Using MATLAB v7.0, an M-file function was created for each of the software bulletins
found in Figure 5.1. The actual MATLAB code of the simulator can be found in Appendix
C. To run the simulation, the user must first input the vehicle parameters such as the
dimensions of the vehicle, initial front wheel speeds, vehicle pose, simulation length, and
timestep of the simulation. Each of the initial input parameters for the simulation and
, RF
their values are listed in Table 5.1. Note that the front wheel speeds (LF ) and body
28
Figure 5.1: Software architecture for the kinematic simulation.
angles ( , ) are considered variables, which allows the ability to change the speed and
direction of the front wheels at each iteration.
Parameter Description Value Type

LF Front left wheel speeds 1 m/s Variable

RF Front right wheel speeds 1 m/s Variable
Intial angle of the vehicle in global space 0 rad Variable
Intial angle of the rear body, relative to the front body 0 rad Variable
t Timestep of simulation iteration 0.5 s Constant
siml ength Total time to run simulation 34 s Constant
CA f Vehicle origin in global space [0, 0] m Constant
width f Distance between front wheel and center of axle 0.600 m Constant
widthr Distance between rear wheel and center of axle 0.521 m Constant
rfr Radius of the front right wheel 0.315 m Constant
rfl Radius of the front left wheel 0.315 m Constant
rrr Radius of the rear right wheel 0.315 m Constant
rrl Radius of the rear left wheel 0.315 m Constant
LO f Distance between front axle and link 1 0.236 m Constant
LOr Distance between rear axle and link 4 0.531 m Constant
link1 Distance from center of link to joint 0.669 m Constant
Table 5.1: Simulation Initial Conditions and Parameters.
29
The simulator uses these initial parameters to analyze the characteristics of the four-
bar configuration. Using the equations derived in Chapter 4, the position and orientation
of the links can be determined along with the position of crossing point (P). For debugging
purposes, the configuration of the linkages is plotted in the four-bar frame (CL1 ) to ensure
the motion of the linkages coincide with the motion of the front and rear bodies. Figure
5.2 illustrates the linkage configuration when the front body angle = 0 rad and the
rear body angle = 1.047 rad. In this figure, the red line represents link1, the blue line
represents link2, the green line represents link4 and the black line represents link3.
Figure 5.2: Plot of the linkage configuration when = 0 rad and = 1.047 rad (link1 =
red, link2 = blue, link3 = black, link4 = green).
Next, using the current position and orientation of the vehicle, the configuration of the
vehicle is plotted in the global frame for each timestep. Figure 5.3 illustrates the vehicle
position and orientation in the global coordinate frame. The plot begins with the center
of the front axle (CA f ) at zero in the global frame, while the front body angle = 0 rad
and the rear body angle = 0 rad. The simulation is iterated every 1 second for a total of
30 seconds. The velocities of the front left and right wheels are (1 rev/s) and (0.5 rev/s)
respectively. The front axle and wheels are illustrated in red and (x), while the rear axle
and wheels are illustrated in blue and (o). The linkage configuration connecting the two
30
bodies is illustrated in black.
Figure 5.3: Plot of the vehicle position in the global coordinate frame. Front wheels
in red, rear wheels in blue, and four-bar linkage in black (t = 1 s, sim length = 34 s,
LF = 1 rev/s, RF = 0.5 rev/s).
Finally, the velocities of the front and rear wheels are determined by iterating the
simulation. Again, using the equations derived in Chapter 4, the simulation can predict
the front and rear velocities for the next timestep due to the initial conditions or the values
from the previous iteration. To further analyze the data from the simulation, variables
such as front and rear wheel position and velocities are logged. To give a more concise
visualization of the path taken by the front and rear wheels, a plot is made from the logged
data indicating the path of the vehicle in the global frame (Figure 5.4). Using the same
parameters as in Figure 5.3, the path of the front and rear wheels are respectively displayed
in red and blue. Note that the erratic discontinuity in the left rear wheel at (3.6 m, 1.8 m)
is caused by the left wheel slowing down to allow the rear body to swing around and align
with the front body. The large timestep (1 s) chosen to iterate the simulation causes this
31
discontinuity to appear larger than what would be observed in real time. Using a smaller
timestep in the simulation would produce a smoother transition in the trajectory of the left
rear wheel.
Figure 5.4: Plot of the trajectory taken by the vehicles wheels in the global coordinate
frame. Trajectory of front wheels in red and rear wheels in blue (t = 1 s, sim length =
34 s, LF = 1 rev/s, RF = 0.5 rev/s).
5.2 Performance
As a means of analyzing the performance of the kinematic simulation and the linkage
design, the simulation described above is compared to a similar kinematic model created
by Sean Baity which was validated on the 2005 Polaris vehicle [3]. For this first test, the
outputs from both models will be compared by using the same vehicle parameters and
similar linkage configurations as presented in [3] to validate the model presented in this
thesis. Finally, a second test will analyze the performance of the linkage design of the
32
Polaris II vehicle; it will be compared to the linkage design of the 2005 Polaris vehicle.
5.2.1 Kinematic Verification
To compare the performance of the two simulations, the linkage configuration must first
be altered to match the single link kingpin joint of the 2005 Polaris vehicle (Figure 1.1).
By uncrossing link2 and link4, while setting the length of link1 very small (0.00001 m),
the kinematic model will have the same characteristics as the single revolute joint pre-
sented in [3]. The lengths of link2 (0.5154 m), link3 (0.5 m), and link4 (0.5154 m) were
chosen to make the total vehicle length the same as in Baitys work (0.5 m). Figure 5.5
shows an illustration of each vehicles linkage for the two simulations.
(a) (b)
Figure 5.5: Plot of vehicle parameters using Baitys simulation (a) and plot of vehicle
parameters using this simulation (b).
Using a simulation length of 79 seconds and a timestep of 1.0 second, both simu-
lations are iterated with the front left and right wheel speeds at 1 rev/s and 0.5 rev/s
for a period of 34 seconds. Then, the front left and right wheel speeds are changed to
0.5 rev/s and 1 rev/s for a period of 67 seconds. The remaining 12 seconds the front left
and right wheel velocities are equal (1 rev/s) to allow the rear body to approach steady
33
state. This arrangement of wheel speeds will cause the vehicle to negotiate an s-curve
and demonstrate a case of vehicle off-tracking. Figure 5.6 uses Baitys simulation to plot
the vehicles position and wheel trajectory within the global coordinate frame. Figure
5.7 uses the simulation from this thesis to plot the vehicles position and wheel trajectory
within the global coordinate frame.
(a) (b)
Figure 5.6: Plot of vehicle position from Baitys simulation (a) wheel trajectory using
Baitys simulation (b).
To analyze the data from both simulations, the wheel velocities from each wheel are
plotted verses time (Figure 5.8). The wheel velocities from Baitys simulation are plotted
in black, while the front and rear wheels are respectively plotted in red and blue. A scatter
plot of the residuals from the velocity analysis indicates the correlation between Baitys
kinematic model and the kinematic model presented in this thesis for identical front body
trajectories (Figure 5.9).
34
(a) (b)
Figure 5.7: Plot of vehicle position from the simulation from this paper (a) wheel trajec-
tory using the simulation developed in this thesis (b).
Figure 5.8: Plot of wheel velocities versus time from both simulations. Baitys wheel
velocities are in black. Front and rear velocities from the simulation developed in this
thesis are in red and blue.
35
Figure 5.9: Scatter plot of the residuals from the wheel velocities.
5.2.2 Linkage Verification
To analyze the linkage design for the Polaris II vehicle, the four-bar linkage will be com-
pared to the linkage of the 2005 Polaris vehicle. Using the same simulation parameters
and vehicle dimensions for the simulation presented in this thesis and the simulation cre-
ated by Sean Baity, the off-tracking amount from each vehicle can be compared. The
parameters for the vehicle dimensions as stated in Table 5.1 are used to configure the two
vehicles. Figure 5.10 shows an illustration of each vehicles linkage for the two simula-
tions.
Due to the larger vehicle size as compared to the parameters for the previous test,
the total simulation time and timed intervals for the wheel speeds needed to be modified
to fit the s-curve model. Using a simulation length of 63 seconds and a timestep of 1
second, both simulations are iterated with the front left and right wheel speeds at 1 rev/s
and 0.5 rev/s for a period of 26 seconds. Then, the front left and right wheel speeds
36
(a) (b)
Figure 5.10: Plot of vehicle parameters using Baitys simulation (a) and plot of vehicle
parameters using this simulation (b).
are changed to 0.5 m/s and 1 m/s for a period of 51 seconds. For The remaining 12
seconds the front left and right wheel velocities are equal to 1 rev/s to allow the rear
body to approach steady state. Figure 5.11 uses Baitys simulation to plot the vehicles
position and wheel trajectory within the global coordinate frame. Figure 5.12 uses the
simulation from this thesis to plot the vehicles position and wheel trajectory within the
global coordinate frame.
To analyze the data from both simulations, the wheel velocities from each wheel are
plotted verses time (Figure 5.13). The wheel velocities from Baitys simulation are plotted
in black, while the front and rear wheels are respectively plotted in red and blue. A scatter
plot of the residuals from the velocity analysis (Figure 5.14) indicates the correlation
between the front wheel velocities for the two simulations. However, the residuals for the
rear wheel velocities indicate a difference between the two models with a mean difference
of 0.4375 m/s for the left wheel and 0.4398 m/s for the right wheel. This can be clearly
seen in the plots of the wheel velocities for the two simulations (Figure 5.13). Also, the
maximum off-tracking distance of the rear wheels from each simulation can be found
37
(a) (b)
Figure 5.11: Plot of vehicle position from Baitys simulation (a) wheel trajectory using
Baitys simulation (b).
(a) (b)
Figure 5.12: Plot of vehicle position from the simulation from this paper (a) wheel trajec-
tory using the simulation developed in this paper (b).
by inspection of Figure 5.11, and Figure 5.12. The maximum off-tracking from Figure
5.11 is approximately 0.375 meters, while the maximum off-tracking from Figure 5.12 is
approximately 0.157 meters.
38
Figure 5.13: Plot of wheel velocities verses time from both simulations. Baitys wheel
velocities are in black. Front and rear velocities from the simulation developed in this
thesis are in red and blue.
Figure 5.14: Scatter plot of the residuals from the wheel velocities.
39
Chapter 6
Conclusion
6.1 Conclusion
The undesirable phenomenon of vehicle off-tracking is a noticable problem in many forms

of transportation. The need for a trailer system to follow the path of the towing vehicle
is apparent in many areas such as robotics, industry, and public transportation. While it
is possible to plan a path that swings the vehicle out around an obstacle, this increases
the vehicles path width and complicates path planning. The focus of this research was
to find a mechanical solution to this off-tracking phenomenon. This was obtained by uti-
lizing the kinematic advantages of a four-bar linkage. The kinematic model developed in
this research was designed to be universal and pertinent to many different linkage configu-
rations, allowing the model to be applied to a countless number of linkage arrangements.
Furthermore, the kinematic model presented here extends the kinematic knowledge of
mobile robotics when applied to an articulating two-bodied vehicle. Even though the
40
original application for this research was for a robotic platform, this research could be
employed in numerous applications involving trailered systems.
The kinematic model was first validated in simulation, which allowed many different
types of linkage scenarios to be tested. The simulation was designed to be user friendly
by only requiring the user to change the initial parameters of the vehicle to change the
simulation environment. Using only the front wheel velocities and initial state of the
body angles, the kinematic model calculates the rear wheel velocities and body pose. This
leads to the implication of a four-wheel drive robotic vehicle. In addition, by changing
the length of one of the links in the system, the trajectory of the vehicles rear body can
be altered. This implies that you could dynamically control the motion of the rear body
by changing the length of one of the links in the four-bar linkage.
The simulation was initially verified by comparing its outputs to a similar simulation
that was validated on the 2005 Polaris vehicle. The simulation was further quantified by
applying the linkage design to a full-scale robotic platform, Polaris II. The inherent mo-
tion of the robotic vehicle corresponded with the simulated results found. By minimizing
the off-tracking of the vehicles rear body, the navigation and control algorithms could be
developed to be oblivious of the vehicles rear section.
6.2 Future Work
6.2.1 Linkage Design
The linkage designed for the Polaris II vehicle incorporated many design features, such
as the second degree of freedom to allow body roll between the front and rear sections;
this may be undesirable in other applications. To incorporate this linkage design into other
applications, an assessment of the linkages deliverables must be considered. Also, the
timeline of the design and construction of the linkage, along with minimal manufacturing
41
capabilities, hindered the strength and stability of the linkage. Further development on the
linkage may incorporate stronger links and roller bearing contacts at the joints to reduce
any flex or pitch between those two bodies.
6.2.2 Four-Wheel Drive Vehicle
Using the velocities calculated for the rear body and outfitting the Polaris II platform with
motors on the rear wheels, the vehicle could be converted into a four-wheel drive vehicle
for traversing difficult terrain. Using the commanded velocities of the front wheels, the
calculated rear wheel velocities could be used to command the rear wheels, allowing rear
body propulsion. This could be accomplished in real time by compiling a look-up table
of rear wheel velocities for given front wheel velocities and linkage configuration.
6.2.3 Dynamically Controlled Off-Tracking
It is shown that changing the link length of one of the links in the four-bar linkage will
change the tracking path of the rear body. Likewise, it is shown for a known linkage
arrangement, the path of the rear body can be predicted. Subsequently, these traits could
be utilized by a sophisticated path-planning algorithm to dynamically control the motion
of the rear section by changing a link length in the four-bar linkage. This could be useful
in situations where the trailing vehicle needs to swing outside or inside the path of the
front vehicle.
6.2.4 Second Degree Of Freedom
The second degree-of-freedom motion that allows roll between the two bodies of Polaris II
dismisses the assumption of having planar motion within the kinematic model. This sec-
ond degree-of-freedom changes the distance between front and rear wheels and the terrain
42
they interact with. To have an accurate kinematic model that would be applicable to var-
ied terrain, this second degree-of-freedom motion would need to be incorporated into the
kinematic model.
43
References
[1] Aims, J., Delpheh, S., Farmer, J., Fries, A., King, P., Konopnicki, K., McElhaney,
C., Pinsker, M., Smith, M., Wang, P., Polaris II, Design Report (Blacksburg, VA:
Virginia Tech, 2007). Available Online: http://www.igvc.org/
[2] Andre, Jean-Luc, Coupling For Controlling The Angular Offset Between Two
Road Transport Units Connected To One Another By A Self-Supporting In-
termediate Unit, U.S. Patent 5,860,667, Jan. 19, 1999. Available Online:
http://www.freepatentsonline.com/.
[3] Baity, Sean M., Development of a Next-generation Experimentation Robotic Vehi-

cle (NERV) that Supports Intelligent and Autonomous Systems Research, masters
thesis, (Blacksburg, VA: Virginia Tech, Dec. 2005).
[4] Baity, S., Gombar, B., Felps, J., Rigsby, S., Greene, J., Oatley, D., Todd, C.,
Miller, N., Johnson, T., Blodgett, J., Weekley, Eargle, D., Mott, B., Avitabile., M.,
Polaris, Design Report (Blacksburg, VA: Virginia Tech, 2005). Available Online:
http://www.igvc.org/.
[5] DellaMoretta, Leonard B., Adjustable Two-Bar Linkage For A Four-

Wheel Trailer, U.S. Patent 4,437,680, Sep. 28, 1982. Available Online:
[6] Deng, Weiwen, et.al. Vehicle-Trailer Low-Speed Offtracking Control, U.S. Patent
US20070152424, July 5, 2007. Available Online: http://www.freepatentsonline.com/.
[7] Elefteriadou, L., Glauz, W.D., Richard, K.R., Torbic, D.J., Harwood, D.W., Review
Of Truck Characteristics as Factors in Road Design, Transportation Research Board
Of The National Academies, NCHRP Report 505, (Washington, DC, 2003).
[8] Ervin, R.D., Fancher, P.S., Gillespie, T.D., An Overview Of The Dynamic Perfor-
mance Properties of Long Truck Combinations, Special Report,(Ann Arbor, Michi-
gan: University of Michigan - Transportation Research Institute, July, 1984).
[9] Feuvray, Alain, Fastening Device For A Trailer And Trailer Comprising
Such A Device, U.S. Patent 4,429,894, Feb. 7, 1984. Available Online:
44
[10] Grove, C.,D., Coupling Means For Dollys, Trailers, And The Like, U.S. Patent
2,313,235, March 9, 1943. Available Online: http://www.freepatentsonline.com/.
[11] Intelligent Ground Vehicle Competition, http://www.igvc.org/.
[12] Jennings, Frederick R., Moritz, James O., True Tracking Trailer, U.S. Patent
4,127,202, Nov. 28, 1978. Available Online: http://www.freepatentsonline.com/.
[13] Kelly, Alonzo. Essential Kinematics for Autonomous Vehicles, masters thesis,
(Pittsburgh, PA: Carnegie Mellon University, May 1994).
[14] Mabie, Hamilton H., Reinholtz, Charles F., Mechanisms And Dynamics Of
Machinery, Fourth Ed., (New York John Whiley & Sons, 1987).
[15] Meadows, Leon Tow Bar, U.S. Patent 5,775,714, July 7, 1998. Available Online:
[16] Propst, Robert,L., Steering Apparatus For A Trailer Vehicle, U.S. Patent
4,134,601, Jan. 16, 1979. Available Online: http://www.freepatentsonline.com/.
[17] Ramirez, R. A., Computer Vision Based Analysis of Broccoli for Application in
a Selective Autonomous Harvester, masters thesis, (Blacksburg, VA: Virginia Tech,
July 2006).
[18] Raymond, M., Leatherman, S., Mills, J., Ceperley, E., Domme, D., Romanelli,
M., Proposal for the Design of Acoustic Arrays for Autonomous Vehicles, Pro-
posal, (Blacksburg, VA: Mechanical Engineering Department,Virginia Tech, October,
2005).
[19] Siegwart, R., Nourbakhsh, I.R., Introduction to Autonomous Mobile Robots, the
MIT Press, (Cambridge, MA, 2004), pg. 61 - 64.
[20] White, Donald, Trailer Articulation Device, U.S. Patent 5,531,468, July 2, 1996.
Available Online: http://www.freepatentsonline.com/.
45
Appendix A
Analytical Position Analysis for

Four-Bar Linkage
Known:
link1 = r1 , link2 = r2 , link3 = r3 , link4 = r4 , = 3
Find:
2
Solve for 2 :
r1 + r2 e j2 + r3 e j3 + r4 e j4 = 0 (A.1)
r1 + r2 cos (2 ) + r3 cos (3 ) r4 cos (4 ) = 0

r2 sin (2 ) + r3 sin (3 ) r4 sin (4 ) = 0
r1 + r2 cos (2 ) + r3 cos (3 ) = r4 cos (4 )

r2 sin (2 ) + r3 sin (3 ) = r4 sin (4 )
(A.2)
Square both sides and add them together:
46
[r1 + r2 cos (2 ) + r3 cos (3 )]2 = [r4 cos (4 )]2
[r2 sin (2 ) + r3 sin (3 )]2 = [r4 sin (4 )]2
r1 2 + 2r1 r2 cos (2 ) + 2r1 r3 cos (3 ) + 2r2 r3 cos (2 ) cos (3 ) + 2r2 r3 sin (2 ) sin (3 ) + ...
r2 2 cos2 (2 ) + r3 2 cos2 (3 ) + r2 2 sin2 (2 ) + r3 2 sin2 (3 ) = r4 2 cos2 (4 ) + r4 2 sin2 (4 )
(A.3)
Use trig identity

cos2 ( ) + sin2 ( ) = 1
r1 2 + r2 2 + r3 2 + 2r1 r2 cos (2 ) + 2r1 r3 cos (3 ) + 2r2 r3 cos (2 ) cos (3 ) + ...

2r2 r3 sin (2 ) sin (3 ) = r4 2 (A.4)
Use trig identity:
cos (a b) = cos (a) cos (b) + sin (a) sin (b)
r1 2 + r2 2 + r3 2 + 2r1 r2 cos (2 ) + 2r1 r3 cos (3 ) + 2r2 r3 (cos (2 3 )) = r4 2

(A.5)
Substitute to simplify:
U = r1 2 + r2 2 + r3 2 + 2r1 2 r3 2 cos (3 )
V= 2r1 r2 + 2r2 r3 cos (3 )
W= 2r2 r3 sin (3 )
U + (V +W ) cos (2 ) +W sin (2 ) = r4 2
(A.6)
Use trig identity, Tangent Half-Angle Formula [14]

2 2
1tan2 2 2 tan 2
t = tan 22 , cos (2 ) = 2

2 , sin (2 ) = 2

2
1tan 2 1+tan 2
1 t2

2t
U + (V +W ) +W = r4 2
1 + t2 1 + t2
47
U(1 + t 2 ) + (V +W )(1 t 2 ) +W (2t) = r4 2 (1 + t 2 )
U(1 + t 2 ) + (V +W )(1 t 2 ) +W (2t) r4 2 (1 + t 2 ) = 0
(U V W r4 2 )t 2 + 2V (t) + (U +V +W r4 2 ) = 0 (A.7)
Substitute to simplify:
A = (U V W r4 2 ), B = 2V (t), C = (U +V +W r4 2 )
At 2 + Bt +C = 0

22 2
A tan + B tan +C = 0
2 2
p
B B2 4(A)(C)

2
tan =
2 2(A)

B B2 4(A)(C)
2 = 2 arctan 2(A) (A.8)
48
Appendix B
Analytical Solution Of Crossing Point P
x2 = x2(2) x2(1) , y2 = y2(2) y2(1)

x4 = x4(2) x4(1) , y4 = y4(2) y4(1)
y2 y4
m2 = , m4 =
x2 x4
Point Slope Formula:
y y0 = m(x x0 ) (B.1)
49
y2 y0 = m2 (x2 x0 )
y4 y0 = m4 (x4 x0 )
m2 x0 y0 = m2 x2 y2
m4 x0 y0 = m4 x4 y4 (B.2)
Rearrange in matrix form:

x0 m2 1 m2 x2 y2
=
y0 m4 1 m4 x4 y4
1
x0 m2 1 m2 x2 y2
= (B.3)
y0 m4 1 m4 x4 y4
50
Appendix C
MATLAB Simulation Code
C.1 Simulation Parameters

%4 Bar Simulation of a 2 Body Vehicle
%Jesse Farmer
%2/27/07
close all;
clear all;
clc;
%################ Vehicle Parameters and Initial Conditions #############

rfr = .3150; %Radius of the front right wheel, meters
rfl = .3150; %Radius of the front left wheel, meters
rrr = .3150; %Radius of the rear right wheel, meters
rrl = .3150; %Radius of the rear left wheel, meters
theta = 0*(pi/180); %intial angle of the vehicle in global space, degrees
gamma = 0*(pi/180); %initial angle between the front section robot frame,
%degrees and the rear section
CA_f = [0;0]; %vehicle origin in global space
width_f = .6004; %distance between the front wheel and the center of axle
%(width/2), meters
length_f = .2362; %distance between center of axle and front body frame
% (length/2), meters
width_r = .5216; %distance between rear wheel and center of axle
% (width/2), meters
length_r = .3429; % distance between center axle and rear body frame
% (length/2), meters
wheel_speed_fl = 1; %rotational speed of the left wheel, radians/sec
51
wheel_speed_fr = 1; %rotational speed of the right wheel, radians/sec
LO_f = -.2362; %negative Y distance between the front axle and the
% joints of link1, meters
LO_r = .5315; %positive Y distance between the rear axle and the
% joints of link3, meters
%%%%%%%%%%%%%%%%A positive value will cross the 4-Bar%%%%%%%%%%%%%%%%%%%%%

link1 = .6693; %measured from center of link to joint, meters
%link1 = .00001;
%link1 = -.1;
link3 = .6693; %measured from center of link to joint, meters
link2 = .8661; %measured as lenght of link, meters
link4 = .8661; %measured as length of link, meters
% ++++++ Simulation Model Parameters ++++++

delta_t = 1; %seconds
%sim_length = 59; %number of seconds to run the sim
sim_length = 31;
%ii = 0;
kk = 1;
time=0:delta_t:sim_length;
for ii = 1:length(time)
%disp(-----------)
% Call 4Bar_Analysis to determin positon of linkages

[theta2,theta4,link2_1,link2_2,link4_1,link4_2,link3_2,Pf,phi_Pf,...
Pr,phi_Pr,link_D,P,LO_f_1,CA_r,LO_r_1] =...
Four_Bar_Analysis(gamma,theta,link1,link2,link3,link4,LO_r,CA_f);
%Call 4Bar_Plot to plot each itteration of the robots movement

[link2_check,link3_check,link4_check,CA_r,f_r_wheel,f_l_wheel,...
r_r_wheel,r_l_wheel] = Four_Bar_Plot(LO_f,LO_f_1,LO_r_1,gamma,...
theta,link2_1,link4_1,link2_2,link4_2,CA_f,CA_r,P,width_f,...
width_r,length_f,length_r);
%Call Sim_Iteration to calculate the iterative processes

[Phi_dot_f,x_dot,y_dot,theta_dot,Vrr_wheel,Vrl_wheel,gamma_dot,...
P_dot_r,P_dot_mag,P_dot_angle] =...
Sim_Iteration(theta,Pf,phi_Pf,Pr,phi_Pr,wheel_speed_fl,...
wheel_speed_fr,width_f,width_r,rfl,rfr,LO_f,LO_r,gamma);
52
%Log variables for display
Phi_dot_f_log(:,kk) = [ii;Phi_dot_f];
gamma_deg = (180/pi)*gamma;
gamma_log(:,kk)= [ii;gamma_deg];
x_dot_log(:,kk) = [ii;x_dot];
y_dot_log(:,kk) = [ii;y_dot];
theta_deg = theta*(180/pi);
theta_log(:,kk) = [ii;theta_deg];
%R_f_log(:,kk) = [ii;R_f];
%Omega_IC_f_log(:,kk) = [ii;Omega_IC_f];
%K_f_log(:,kk) = [ii;K_f];
P_dot_mag_log(:,kk) = [ii;P_dot_mag];
P_dot_angle_log(:,kk) = [ii;P_dot_angle];
P_dot_r_log(:,kk) = [ii;P_dot_r];
%R_r_log(:,kk) = [ii;R_r];
%Omega_IC_r_log(:,kk) = [ii;Omega_IC_r];
Vrr_wheel_log(:,kk) = [ii;Vrr_wheel];
Vrl_wheel_log(:,kk) = [ii;Vrl_wheel];
gamma_dot_log(:,kk) = [ii;gamma_dot];
% CG_f_log(:,kk) = [ii;CA_f];
% CG_r_log(:,kk) = [ii;CA_r];
f_r_wheel_log(:,kk) = [ii;f_r_wheel];
f_l_wheel_log(:,kk) = [ii;f_l_wheel];
r_r_wheel_log(:,kk) = [ii;r_r_wheel];
r_l_wheel_log(:,kk) = [ii;r_l_wheel];
% Update parameters for iterative analysis

CA_f = CA_f + [x_dot*delta_t; y_dot*delta_t];
theta = theta + theta_dot*delta_t;
gamma = gamma + gamma_dot*delta_t;
% This loop is used to dynamically change the front wheel velocities

% Update step counts
ii = ii + delta_t;
kk = kk + 1;
if ii <= 0
wheel_speed_fl = 1; %rotational speed of the left wheel, rad/sec
wheel_speed_fr = 1; %rotational speed of the right wheel, rad/sec
elseif ii <= 25
wheel_speed_fl = 1;
wheel_speed_fr = .5;
53
else
wheel_speed_fl = 1;
wheel_speed_fr = 1;
end
% if ii <= 6
% wheel_speed_fl = 1; %rotational speed of the left wheel, rad/sec
% wheel_speed_fr = 1; %rotational speed of the right wheel, rad/sec
% elseif ii <= 56
% wheel_speed_fl = 1;
% wheel_speed_fr = .5;
% elseif ii <= 106
% wheel_speed_fl = .5;
% wheel_speed_fr = 1;
% else
% wheel_speed_fl = 1;
% wheel_speed_fr = 1;
% end
% pause
%This is used to plot the vector P_r in the global frame

%Rotate about Z axis
rot_z = [cos(theta),-sin(theta);sin(theta),cos(theta)];
%Translate
translate = [0;LO_f];
P = rot_z*(P + translate) + CA_f;
P_dot_r = rot_z*(P_dot_r + translate) + CA_f;
end
figure(5)
set(gcf, Toolbar, none);
set(gcf, Units, inches);
set(gcf, Position, [0 3 4 4]);
plot(f_r_wheel_log(2,:),f_r_wheel_log(3,:),-r,...
f_l_wheel_log(2,:),f_l_wheel_log(3,:),-r,...
r_r_wheel_log(2,:),r_r_wheel_log(3,:),-b,...
r_l_wheel_log(2,:),r_l_wheel_log(3,:),-b,LineWidth,2.5)
% plot([P(1,1),P_dot_r(1,1)],[P(2,1),P_dot_r(2,1)],-g,LineWidth,2.5)
hold on
title(Front and Rear Wheel Travel,FontSize,14)
ylabel(Global Y Distance, Meters,FontSize,14)
54
xlabel(Global X Distance, Meters,FontSize,14)
axis equal
grid on
% figure(3)
% plot(Vrr_wheel_log(1,:)*delta_t,Vrr_wheel_log(2,:),r)
% hold on
% % plot(Wheel_dot_f_log(1,:)*delta_t,Wheel_dot_f_log(2,:),g)
% % hold on
% plot(Vrl_wheel_log(1,:)*delta_t,Vrl_wheel_log(2,:),b)
% % hold on
% % plot(Wheel_dot_f_log(1,:)*delta_t,Wheel_dot_f_log(3,:),y)
% title(Rear Wheel Velocities)
% ylabel(Velocity, Inches/sec)
% xlabel(Time, sec)
% grid on
C.2 Four-Bar Analysis

%Jesse Farmer
%3/11/07
%----------------------- 4-Bar Analysis ------------------------
% function[theta2,theta4,link2_1,link2_2,link4_1,link4_2,link3_2,Pf,phi_Pf,...
% Pr,phi_Pr,link_D,P,LO_f_1,CA_r,LO_r_1] =...
% Four_Bar_Analysis(gamma,theta,link1,link2,link3,link4,LO_r,CA_f)
%################# 4Bar Analysis for angles theta2, theta4 ###############

theta3 = gamma - theta; %change gamma into the angle theta3 in 4-bar
% coordinate frame
%theta3_deg = theta3*180/pi;
%=======================Kinematic 4Bar Equation===========================

% link1 + link2*ejtheta2 + link3*ejtheta3 - link4*ejtheta4 = 0
%=========================================================================
%Combine elements of the kinematic 4Bar equation to clean up equations

A1 = link12 + 2*link1*link3*cos(theta3) + link22 + link32;
B1 = 2*link1*link2 + 2*link2*link3*cos(theta3);
C1 = 2*link2*link3*sin(theta3);
55
%Combine again to clean up equation
A2 = A1-B1-link42;
B2 = 2*C1;
C2 = A1+B1-link42;
%Use tan half angle identitiy --> t = tan(theta2/2)

%This gives you a quadratic funtion (A2t2 + B2t + C2) to solve theta2
theta2_1 = 2 * atan((-B2 + sqrt(B22 - 4*A2*C2))/(2*A2));
theta2_2 = 2 * atan((-B2 - sqrt(B22 - 4*A2*C2))/(2*A2));
theta2_3 = theta2_1 + pi;
theta2_4 = theta2_2 + pi;
%Plug the values back into the inital 4-Bar equations to find theta4
theta4_1 = atan2((link2*sin(theta2_1) + link3*sin(theta3)),...
(link1 + link2*cos(theta2_1) + link3*cos(theta3)));
%Evaluate each term of theta2 and theta4 to find which combination will
%equate to zero
theta2 = [theta2_1, theta2_2, theta2_3, theta2_4];
theta4 = [theta4_1, theta4_2, theta4_3, theta4_4];
theta24=[theta2;theta4];
for ii = 1:length(theta2)
fourbar_eq_c(ii) = link1 + link2*cos(theta24(1,ii)) + link3*cos(theta3)...
- link4*cos(theta24(2,ii));
fourbar_eq_s(ii) = link2*sin(theta24(1,ii)) + link3*sin(theta3)...
- link4*sin(theta24(2,ii));
if ((abs(fourbar_eq_c(ii))<1e-6) && (abs(fourbar_eq_s(ii))<1e-6))
theta_check(:,ii)=[theta24(1,ii);theta24(2,ii)];
else
theta_check(:,ii)=[0;0];
end
end
%theta_check;
56
% Determine which element in the matrix theta_check, is the value for
% thata2 and theta4
theta2 = theta_check(1,:);
theta2 = theta_check(find(theta2<0)); %use the find command to
% determin values below zero
theta2 = min(theta_check(1,:));
% if theta2 == 0
% theta2 = max(theta_check(1,:));
% else
% theta2 = min(theta_check(1,:));
% end
theta4 = min(theta_check(2,:));
% if theta4 == 0
% theta4 = max(theta_check(2,:));
% else
% theta4 = min(theta_check(2,:));
% end
theta2_Z = theta2;
theta4_Z = theta4;
theta2 = theta2_1;
theta4 = theta4_1;
theta2_deg = theta2*(180/pi);
theta4_deg = theta4*(180/pi);
%+++++++++++++++ Find Instant Center point P and length D ++++++++++++++
%Find the x,y coordinates of link2 and link4

link2_1 = [(link1/2);0];
link2_2 = [(link1/2) + link2*cos(theta2);...
0 + (link2*sin(theta2))];
link4_1 = [-(link1/2);0];
link4_2 = [-(link1/2) + link4*cos(theta4);...
0 + link4*sin(theta4)];
%Find the slope of link2 and link4

y2 = link2_1(2,1) - link2_2(2,1);
x2 = link2_1(1,1) - link2_2(1,1);
y4 = link4_1(2,1) - link4_2(2,1);
x4 = link4_1(1,1) - link4_2(1,1);
57
slope2 = y2/x2;
slope4 = y4/x4;
%Find the line intersect of point P using the point slope formula
%================== y - y0 = m(x - x0) ======================
P = inv([slope2, -1;slope4, -1])*[(slope2*link2_1(1,1) - link2_1(2,1));...
(slope4*link4_1(1,1) - link4_1(2,1))];
%Define the center of link1

LO_f_1 = [0;0];
%Find x,y coordinates of the center of link3
link3_2 = [(link3/2)*cos(theta3);-(link3/2)*sin(theta3)];
%Find the center of link3
LO_r_1 = [(link2_2(1,1)+link4_2(1,1))/2;(link2_2(2,1)+link4_2(2,1))/2];
%LO_r_1 = [link2_2(1,1) + link3_2(1,1);link2_2(2,1) + link3_2(2,1)];
%Find the center of the rear axle
CA_r = [LO_r_1(1,1) + LO_r*sin(theta3);LO_r_1(2,1) - LO_r*cos(theta3)];
%Find the length of vector Pf
Pf = sqrt((P(2,1) - LO_f_1(2,1))2 + (P(1,1) - LO_f_1(1,1))2);
%Find the angle of vector Pf
phi_Pf = atan2((P(2,1) - LO_f_1(2,1)),(P(1,1) - LO_f_1(1,1)));
%phi_Pf_deg = phi_Pf*(180/pi);
%Find length of vector Pr
Pr = sqrt((P(2,1) - LO_r_1(2,1))2 + (P(1,1) - LO_r_1(1,1))2);
%Find the angle of vector Pr
phi_Pr = atan2((P(2,1) - LO_r_1(2,1)),(P(1,1) - (LO_r_1(1,1))))...
+ theta-gamma; %%%changed
%phi_Pr_deg = theta_Pr*(180/pi);
%Find the length of link_D
link_D = sqrt((LO_f_1(2,1) - LO_r_1(2,1))2 + (LO_f_1(1,1) -...
LO_r_1(1,1))2);
% Plot the 4Bar

figure(2)
set(gcf, Position, [8.125 3 4 4]);
plot([link4_1(1,1),link2_1(1,1)],[link4_1(2,1),link2_1(2,1)],*-r,...
[link2_1(1,1),link2_2(1,1)],[link2_1(2,1),link2_2(2,1)],*-b,...
[link2_2(1,1),link4_2(1,1)],[link2_2(2,1),link4_2(2,1)],*-r,...
[link4_2(1,1),link4_1(1,1)],[link4_2(2,1),link4_1(2,1)],*-g,...
[LO_r_1(1,1),CA_r(1,1)],[LO_r_1(2,1),CA_r(2,1)],*-y);
xlabel(4-Bar X-Position, meters,FontSize,14)
58
ylabel(4-Bar Y-Position, meters,FontSize,14)
title(Motion in the 4-Bar Coordinate Frame,FontSize,14)
axis equal
% axis square
% hold on
C.3 Four-Bar Plot

%Jesse Farmer
%3/29/07
%========================== 4-Bar Plot ===========================
% function [link2_check,link3_check,link4_check,CA_r,f_r_wheel,f_l_wheel,...
% r_r_wheel,r_l_wheel] = Four_Bar_Plot(LO_f,LO_f_1,LO_r_1,gamma,...
% theta,link2_1,link4_1,link2_2,link4_2,CA_f,CA_r,P,width_f,...
% width_r,length_f,length_r)
%Rotate and translate each link point into global coordinate frame
%Rotate about Z axis
rot_z = [cos(theta),-sin(theta),;sin(theta),cos(theta)];
%Translate
translate = [0;LO_f];
%Translate and rotate link point into global coordinate frame
link2_1 = rot_z*(link2_1 + translate) + CA_f;
link2_2 = rot_z*(link2_2 + translate) + CA_f;
link4_1 = rot_z*(link4_1 + translate)+ CA_f;
link4_2 = rot_z*(link4_2 + translate)+ CA_f;
%LO_f_1 = rot_z*(LO_f_1 + translate)+ CA_f;
%LO_r_1 = rot_z*(LO_r_1 + translate)+ CA_f;
CA_r = rot_z*(CA_r + translate)+ CA_f;
%P = rot_z*(P + translate)+ CA_f;
%################## Plot the positions of the 4-Bar ######################
%link3_2 = [(link3/2)*cos(gamma);(link3/2)*sin(gamma)];
%************ Need to change this after iterative calculaitons ************
%LO_f_1 = [CA_f(1,1) + -LO_f*sin(theta);CA_f(2,1) + LO_f*cos(theta)];
%**************************************************************************
%LO_r_1 = [link2_2(1,1) + link3_2(1,1);link2_2(2,1) + link3_2(2,1)];
59
%Find the position of the front right and left wheels
f_r_wheel = [(CA_f(1,1) + width_f*cos(theta));(CA_f(2,1) +...
width_f*sin(theta))];
f_l_wheel = [(CA_f(1,1) - width_f*cos(theta));(CA_f(2,1) -...
width_f*sin(theta))];
%Find the position of the rear right and left wheels

r_r_wheel = [(CA_r(1,1) + width_r*cos(gamma));(CA_r(2,1) +...
width_r*sin(gamma))];
r_l_wheel = [(CA_r(1,1) - width_r*cos(gamma));(CA_r(2,1) -...
width_r*sin(gamma))];
%Find the coordinates of corners for the front and rear bodies
%************ X,Y is at 0,0 in the center of the front axle *************
%Top left corner of the front body
%top_left_f = rot_z*[-width_f;length_f]+CA_f;
%Top right corner of the front body
%top_right_f = rot_z*[width_f;length_f]+CA_f;
%Bottom left corner of the front body
%bot_left_f = rot_z*[-width_f;-length_f]+CA_f;
%Bottom left corner of the front body
%bot_right_f = rot_z*[width_f;-length_f]+CA_f;
%************ X,Y is at 0,0 in the center of the rear axle **************

%Rotate the rear body according to the orientation of the front body
%rot_r = [cos(gamma),-sin(gamma);sin(gamma),cos(gamma)];
%Top left corner of the rear body
%top_left_r = CA_r+(rot_r*[-width_r;length_r]);
%Top right corner of the rear body
%top_right_r = CA_r+(rot_r*[width_r;length_r]);
%Bottom left corner of the rear body
%bot_left_r = CA_r+(rot_r*[-width_r;-length_r]);
%Bottom left corner of the rear body
%bot_right_r = CA_r+(rot_r*[width_r;-length_r]);
link2_check = sqrt(((link2_1(1,1) - link2_2(1,1))2) +...

((link2_1(2,1) - (link2_2(2,1)))2));
link4_check = sqrt(((link4_1(1,1) - link4_2(1,1))2) +...
((link4_1(2,1) - (link4_2(2,1)))2));
link3_check = sqrt((link4_2(1,1) - link2_2(1,1))2 + ...
(link4_2(2,1) - link2_2(2,1))2);
60
figure(1)
set(gcf, Position, [4.063 3 4 4]);
hold on
plot([link4_1(1,1),link2_1(1,1)],[link4_1(2,1),link2_1(2,1)],-k,...
[link2_1(1,1),link2_2(1,1)],[link2_1(2,1),link2_2(2,1)],-k,...
[f_r_wheel(1,1),f_l_wheel(1,1)],[f_r_wheel(2,1),f_l_wheel(2,1)],...
x-r,[r_r_wheel(1,1),r_l_wheel(1,1)],[r_r_wheel(2,1),r_l_wheel(2,1)],...
o-b);
axis equal
grid on
xlabel(Global X-Position, meters,FontSize,13)
ylabel(Global Y-Position, meters,FontSize,13)
title(Motion in the Global Inertial Coordinate Frame,FontSize,13)
%axis([-1.25 3.75 -1 4])
%*************************************************************************
% %%%%%%%%%%%%%%%%% Plot with Front and Rear Bodies %%%%%%%%%%%%%%%%%%%%%%
% figure(1)
% hold on
% plot([link4_1(1,1),link2_1(1,1)],[link4_1(2,1),link2_1(2,1)],*-b,...
% [link2_1(1,1),link2_2(1,1)],[link2_1(2,1),link2_2(2,1)],*-c,...
% [link2_2(1,1),link4_2(1,1)],[link2_2(2,1),link4_2(2,1)],*-g,...
% [link4_2(1,1),link4_1(1,1)],[link4_2(2,1),link4_1(2,1)],*-m,...
% [f_r_wheel(1,1),f_l_wheel(1,1)],[f_r_wheel(2,1),f_l_wheel(2,1)],...
% or,[r_r_wheel(1,1),r_l_wheel(1,1)],[r_r_wheel(2,1),r_l_wheel(2,1)],...
% ok,[top_left_f(1,1),top_right_f(1,1)],[top_left_f(2,1),...
% top_right_f(2,1)],-r,[top_left_f(1,1),bot_left_f(1,1)],...
% [top_left_f(2,1),bot_left_f(2,1)],-r,[bot_left_f(1,1),...
% bot_right_f(1,1)],[bot_left_f(2,1),bot_right_f(2,1)],-r,...
% [bot_right_f(1,1),top_right_f(1,1)],[bot_right_f(2,1),...
% top_right_f(2,1)],-r,[top_left_r(1,1),top_right_r(1,1)],...
% [top_left_r(2,1),top_right_r(2,1)],-k,[top_left_r(1,1),...
% bot_left_r(1,1)],[top_left_r(2,1),bot_left_r(2,1)],-k,...
% [bot_left_r(1,1),bot_right_r(1,1)],[bot_left_r(2,1),...
% bot_right_r(2,1)],-k,[bot_right_r(1,1),top_right_r(1,1)],...
% [bot_right_r(2,1),top_right_r(2,1)],-k);
% axis equal
61
% figure(1)
% hold on
% plot([link4_1(1,1),link2_1(1,1)],[link4_1(2,1),link2_1(2,1)],*-r,...
% [link2_1(1,1),link2_2(1,1)],[link2_1(2,1),link2_2(2,1)],*-b,...
% [link2_2(1,1),link4_2(1,1)],[link2_2(2,1),link4_2(2,1)],*-r,...
% [link4_2(1,1),link4_1(1,1)],[link4_2(2,1),link4_1(2,1)],*-g,...
% [LO_f_1(1,1),CA_f(1,1)],[LO_f_1(2,1),CA_f(2,1)],*-y,...
% [LO_r_1(1,1),CA_r(1,1)],[LO_r_1(2,1),CA_r(2,1)],*-y,...
% [CA_f(1,1),P(1,1)],[CA_f(2,1),P(2,1)],o-m,...
% [P(1,1),CA_r(1,1)],[P(2,1),CA_r(2,1)],o-m,...
% [CA_f(1,1),CA_r(1,1)],[CA_f(2,1),CA_r(2,1)],:c,...
% [f_r_wheel(1,1),f_l_wheel(1,1)],[f_r_wheel(2,1),f_l_wheel(2,1)],...
% o-c,[r_r_wheel(1,1),r_l_wheel(1,1)],[r_r_wheel(2,1),...
% r_l_wheel(2,1)],o-c);
% axis equal
% % axis ([-1,1,-1,1])
C.4 Simulation Iteration

%Jesse Farmer
%3/31/07
%###################### Iterative Calculations ##########################
% function[Wheel_dot_f,x_dot,y_dot,theta_dot,Vrr_wheel,Vrl_wheel,...
% gamma_dot,P_dot_r] =...
% Sim_Iteration(theta,Pf,phi_Pf,Pr,phi_Pr,wheel_speed_fl,...
% wheel_speed_fr,width_f,width_r,rfl,rfr,LO_f,LO_r,gamma);
%++++++++++++ Calculate the velocities of the front section +++++++++++++

%Geomtery of body and axle
alpha1 = -pi/2;
alpha2 = pi/2;
beta1 = pi;
beta2 = 0;
%Postion Velocity Matrix
Radi = [rfr 0; 0 rfl];
Wheel_speed=[wheel_speed_fr;wheel_speed_fl];
Wheel_dot_f=[Radi*Wheel_speed;0];
62
%Rolling Constraints
J1f_1 = [-cos(alpha1+beta1) sin(alpha1+beta1) -width_f*cos(beta1)];
J1f_2 = [-cos(alpha2+beta2) sin(alpha2+beta2) -width_f*cos(beta2)];
%Sliding Constraints
C1f1 = [sin(alpha1+beta1) cos(alpha1+beta1) width_f*sin(beta1)];
C1f2 = [sin(alpha2+beta2) cos(alpha2+beta2) width_f*sin(beta2)];
%Constraint matrix
C=inv([J1f_1;J1f_2;C1f1]);
%Rotation Matrix, rotation about z axis to get into global coordinate frame
rot_theta = [cos(theta) -sin(theta) 0 ; sin(theta) cos(theta) 0 ; 0 0 1];
zeta = rot_theta*C*Wheel_dot_f;
%zeta = C*Wheel_dot_f;
x_dot = zeta(1,1);
y_dot = zeta(2,1);
theta_dot = zeta(3,1);
%Make gamma_dot equal theta_dot when front and rear Instant Center is
%infinate
if Wheel_dot_f(1,1) == Wheel_dot_f(2,1) && gamma == theta
%Make the velocity of gamma equal to the velocity of theta when the
%vehicle is going strait ahead
gamma_dot = theta_dot;
Vrr_wheel = Wheel_dot_f(1,1);
Vrl_wheel = Wheel_dot_f(2,1);
%Make the velocity at point P equal to the front wheel velocities at
%the given vector angle of theta_Pr
P_dot_mag = Wheel_dot_f(1,1);
P_dot_angle = phi_Pr - gamma+theta;
P_dot_r = [P_dot_mag*cos(P_dot_angle);P_dot_mag*sin(P_dot_angle)];
% Calculate gamma_dot when Instant Center is at the rear of the vehicle

elseif Wheel_dot_f(1,1) == Wheel_dot_f(2,1) && theta = gamma
%Make the velocity at point P equal to the front wheel velocities at

%the given vector angle of theta_Pr
P_dot_mag = Wheel_dot_f(1,1);
P_dot_angle = phi_Pr - gamma+theta;
%Add the Link Offset LO_r to the vector Pr in the back body frame
P_rear = [0;LO_r] + [Pr*cos(phi_Pr);Pr*sin(phi_Pr)];
if P_dot_angle == pi/2
63
Vrr_wheel = P_dot_r;
Vrl_wheel = P_dot_r;
elseif P_dot_angle == -pi/2
Vrr_wheel = -P_dot_r;
Vrl_wheel = -P_dot_r;
else
%Calculate the rotation velocity of the Instant Center of the rear body
Omega_IC_r = (-P_dot_r(1,1))/(P_rear(2,1));
%Calculate the distance from the CA_r of the rear body to the
%Instant Center
R_r = (P_dot_r(2,1)/Omega_IC_r) - P_rear(1,1);
%Calculate the velocities of the right and left rear wheels

Vrr_wheel = Omega_IC_r*(R_r + width_r);
Vrl_wheel = Omega_IC_r*(R_r - width_r);
end
%Calculate the change in gamma from the rear wheel velocities

gamma_dot = (Vrr_wheel - Vrl_wheel)/(2*width_r);
else
%Calculate the distance from the center of the front body to the
%Instant Center and determin the sign from size of wheel velocities
R_f = (width_f*(Wheel_dot_f(1,1) + Wheel_dot_f(2,1)))/...
(Wheel_dot_f(2,1) - Wheel_dot_f(1,1))*-1;
%Calculate the rotation velocity of the Instant Center of the front body
%Multiply by -1 to keep the angle convention CCW positve and CW negative
if R_f == width_f ;
Omega_IC_f = Wheel_dot_f(2,1)/(R_f - width_f);
% Omega_IC_f = (Wheel_dot_f(1,1)/(R_f + width_f));
elseif wheel_speed_fl == wheel_speed_fr;
Omega_IC_f = 0;
else
Omega_IC_f = (Wheel_dot_f(1,1)/(R_f + width_f));
% Omega_IC_f = Wheel_dot_f(2,1)/(R_f - width_f)*-1;
end
%Calculate the x,y coordinates of link K from point P to the

%Instant Center
64
K_f = [(R_f + Pf*cos(phi_Pf));(Pf*sin(phi_Pf)+ LO_f)];
%theta_K_f = atan2(K_f(2,1),K_f(1,1));
%Find the velocity of point P
%P_dot = Omega_IC_f*K_f;
P_dot = cross([0;0;Omega_IC_f],[K_f;0]);
%+++++++++++ Calculate the velocities of the rear section ++++++++++++
%Rotate and add the LO_r to vector Pr

%P_rear = [cos(gamma-theta),-sin(gamma-theta);sin(gamma-theta),...
%cos(gamma-theta)]*[0;LO_r] + [Pr*cos(phi_Pr);Pr*sin(phi_Pr)];
%Add the Link Offset LO_r to the vector Pr in the back body frame
P_rear = [0;LO_r] + [Pr*cos(phi_Pr);Pr*sin(phi_Pr)];
%Calculate the P_dot according to the rear body

P_dot_mag = sqrt(P_dot(1,1)2 + P_dot(2,1)2);
P_dot_angle = atan2(P_dot(2,1),P_dot(1,1)) - gamma + theta;
if P_dot_angle == pi/2
Vrr_wheel = P_dot_mag;
Vrl_wheel = P_dot_mag;
elseif P_dot_angle == -pi/2
Vrr_wheel = -P_dot_mag;
Vrl_wheel = -P_dot_mag;
else
%Calculate the rotation velocity of the Instant Center of the rear body
Omega_IC_r = (-P_dot_r(1,1))/(P_rear(2,1));
%Calculate the distance from the CA_r of the rear body to the
%Instant Center
R_r = (P_dot_r(2,1)/Omega_IC_r) - P_rear(1,1);
%Calculate the velocities of the right and left rear wheels

Vrr_wheel = Omega_IC_r*(R_r + width_r)
Vrl_wheel = Omega_IC_r*(R_r - width_r)
%Wheel_dot_r = [Vrr_wheel;Vrl_wheel;0];
end
%Vrr_wheel
% Vrl_wheel
%Determin the velocity of the center point of the rear axel
65
%V_CG_r = (Vrr_wheel + Vrl_wheel)/2;
%V_CG_r = C*Wheel_dot_r;
%%%%Peters trying somehting here

% Wheel_dot_r = [Vrr_wheel;Vrl_wheel;0];
% rot_theta = [cos(gamma) -sin(gamma) 0 ;...
% sin(gamma) cos(gamma) 0 ; 0 0 1];
% epsilon_r = rot_theta*C*Wheel_dot_r;
%Calculate the change in gamma from the rear wheel velocities

gamma_dot = (Vrr_wheel - Vrl_wheel)/(2*width_r);
end
66

Thesis JLF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Thesis JLF

Uploaded by

Copyright:

Available Formats

Kinematic Analysis Of A Two-Body Articulated

Jesse Lee Farmer

Dr. Charles F. Reinholtz, Chairman

Dr. Alfred L. Wicks, Co-Chairman

Dr. Dennis W. Hong

Keywords: kinematic model, four-bar linkage, vehicle off-tracking

Jesse Lee Farmer

The kinematic analysis of an articulated twin-body, four-wheel, robotic vehicle is pre-

Chapter 2: Literature Review 5

Chapter 3: Base Vehicle 8

Chapter 4: Kinematic Model 15

Chapter 5: Software Development 28

Appendix A: Analytical Position Analysis for Four-Bar Linkage 46

Appendix B: Analytical Solution Of Crossing Point P 49

Appendix C: MATLAB Simulation Code 51

1.1 Setup for acoustic array data collection. . . . . . . . . . . . . . . . . . . 2

2.1 Illustration of low-speed truck off-tracking. . . . . . . . . . . . . . . . . 6

5.1 Software architecture for the kinematic simulation. . . . . . . . . . . . . 29

3.1 Summary of Sensors Used In The 2007 IGVC Events. . . . . . . . . . . . 14

4.1 Variables Used For Front Body Parameters. . . . . . . . . . . . . . . . . 19

5.1 Simulation Initial Conditions and Parameters. . . . . . . . . . . . . . . . 29

Figure 1.1: Setup for acoustic array data collection (2006).

1.2 Thesis Overview

2.1 Vehicle Off-Tracking

Figure 2.1: Illustration of low-speed truck off-tracking. [7]

2.2 Patent Designs

3.1 Intelligent Ground Vehicle Competition

3.2 Vehicle Overview

3.2.1 Chassis and Drive Train

3.2.2 Hardware and Electrical Components

Figure 3.2: Polariss system architecture for 2007 IGVC events.

4.2 Front Body

the motion of the front body within the global frame.

Figure 4.2: Illustration of the constraints on the front body.

Table 4.1: Variables Used For Front Body Parameters.

The sliding constraint is expressed in the global frame as

4.3 Four-Bar Linkage

4.3.1 Position Analysis

link1 + link2 e j2 + link3 e j3 + link4 e j4 = 0 (4.10)

4.3.2 Velocity Analysis

Velocity Of The Four-Bar

Velocity Of The Rear Body

V~p = jr Kr e jKr (4.17)

Kr e jKr = Rr + Pr e jPr (4.18)

V~r r = r (Rr + lr ) (4.22)

5.1 MATLAB Code

Parameter Description Value Type

Table 5.1: Simulation Initial Conditions and Parameters.

5.2.1 Kinematic Verification

5.2.2 Linkage Verification

The undesirable phenomenon of vehicle off-tracking is a noticable problem in many forms

6.2 Future Work

6.2.1 Linkage Design

6.2.2 Four-Wheel Drive Vehicle

6.2.3 Dynamically Controlled Off-Tracking

6.2.4 Second Degree Of Freedom

[3] Baity, Sean M., Development of a Next-generation Experimentation Robotic Vehi-

[5] DellaMoretta, Leonard B., Adjustable Two-Bar Linkage For A Four-

[11] Intelligent Ground Vehicle Competition, http://www.igvc.org/.

Analytical Position Analysis for

link1 = r1 , link2 = r2 , link3 = r3 , link4 = r4 , = 3

r1 + r2 cos (2 ) + r3 cos (3 ) r4 cos (4 ) = 0

r1 + r2 cos (2 ) + r3 cos (3 ) = r4 cos (4 )

% X,Y is at 0,0 in the center of the rear axle **