Kinematic Analysis Of A Two-Body ArticulatedRobotic Vehicle

Jesse Lee Farmer

Thesis submitted to the faculty of the Virginia Polytechnic Institute and StateUniversity in partial fulfillment of the requirements for the degree of

Master of Sciencein

Mechanical Engineering

Dr. Charles F. Reinholtz, ChairmanAlumni Distinguished Professor, Virginia Tech

Chair, Mechanical Engineering, Embry Riddle Aeronautical University

Dr. Alfred L. Wicks, Co-ChairmanAssociate Professor of Mechanical Engineering, Virginia Tech

Dr. Dennis W. HongAssistant Professor of Mechanical Engineering, Virginia Tech

February 8, 2008Blacksburg, 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 IntelligentGround Vehicle Competition (IGVC) at Virginia Tech, was redesigned in 2006 to improvethe mobility of the vehicle by incorporating an innovative four-bar linkage that connectsthe two bodies. The new linkage design minimizes vehicle off-tracking by allowing therear 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, differentiallydriven vehicle. The kinematic model is validated through computer simulation as well asexperimentation on a fully operational robotic vehicle. The kinematic model presentedhere outlines the foundations for an autonomous, four-wheel drive, multi-body controlsystem and opens avenues for dynamically controlling the tracking of the vehicle’s rearbody with an actuated linkage configuration.

AcknowledgmentsThis 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 givingme strength and guidance throughout my life. I would also like to thank my family andfriends 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. DennisHong. Not only have you been my mentors throughout my graduate career, you have beengreat friends. The camaraderie you have with your students both during and after schoolis irreplaceable. Thank you for taking me under you wing and letting me be a part of thisexperience.

Next, I would like to give credit to the 2007 Virginia Tech IGVC team for all of theirhard 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 Kingand Jon Weekly. Peter, thank you for all your help and long hours spent scratching ourheads on the kinematic simulation. Without your help I believe I would still be sittingin the WARE lab with no hair and no solution. Jon, thank you for all your help withLabVIEW 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 theirguidance and expertise on the 2007 IGVC team, but for their work and consultation onthe 2007 DARPA Urban Challenge.

Finally, I would like to give thanks to my mentor and friends from Bluefield StateCollege, Dr. Bob Riggins and his wife Debbie Riggins. Thank you for sparking myinterest in robotics as well as providing me with the knowledge and desire to pursue ahigher education.

iii

Contents

Chapter 1: Introduction 11.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Thesis Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

Chapter 2: Literature Review 52.1 Vehicle Off-Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 Patent Designs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

Chapter 3: Base Vehicle 83.1 Intelligent Ground Vehicle Competition . . . . . . . . . . . . . . . . . . 83.2 Vehicle Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.2.1 Chassis and Drive Train . . . . . . . . . . . . . . . . . . . . . . 103.2.2 Hardware and Electrical Components . . . . . . . . . . . . . . . 113.2.3 Sensors and Computing . . . . . . . . . . . . . . . . . . . . . . 12

3.3 Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

Chapter 4: Kinematic Model 154.1 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164.2 Front Body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.3 Four-Bar Linkage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

4.3.1 Position Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 214.3.2 Velocity Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 23

Chapter 5: Software Development 285.1 MATLAB Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285.2 Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

5.2.1 Kinematic Verification . . . . . . . . . . . . . . . . . . . . . . . 335.2.2 Linkage Verification . . . . . . . . . . . . . . . . . . . . . . . . 36

iv

Chapter 6: Conclusion 406.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 406.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

6.2.1 Linkage Design . . . . . . . . . . . . . . . . . . . . . . . . . . . 416.2.2 Four-Wheel Drive Vehicle . . . . . . . . . . . . . . . . . . . . . 426.2.3 Dynamically Controlled Off-Tracking . . . . . . . . . . . . . . . 426.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 51C.1 Simulation Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . 51C.2 Four-Bar Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55C.3 Four-Bar Plot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59C.4 Simulation Iteration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

v

List of Figures

1.1 Setup for acoustic array data collection. . . . . . . . . . . . . . . . . . . 21.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 Polaris’s system architecture for 2007 IGVC events. . . . . . . . . . . . . 13

4.1 Illustration of the vehicles coordinate frames within the global frame. . . 164.2 Illustration of the constraints on the front body. . . . . . . . . . . . . . . 184.3 Illustration of the four-bar linkage frame in the front body frame. . . . . . 224.4 Illustration of the instantaneous center of point (P) with respect to the

front body. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 244.5 Illustration of the instantaneous center of the rear body with respect to

point P. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5.1 Software architecture for the kinematic simulation. . . . . . . . . . . . . 295.2 Plot of the linkage configuration. . . . . . . . . . . . . . . . . . . . . . . 305.3 Plot of the vehicle position in the global coordinate frame. . . . . . . . . 315.4 Plot of the trajectory taken by the vehicle’s wheels in the global coordinate

frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325.5 Plot of vehicle parameters using Baity’s simulation (a) and plot of vehicle

parameters using this simulation (b). . . . . . . . . . . . . . . . . . . . . 335.6 Plot of vehicle position from Baity’s simulation (a) wheel trajectory using

Baity’s simulation (b). . . . . . . . . . . . . . . . . . . . . . . . . . . . 345.7 Plot of vehicle position from the simulation from this paper (a) wheel

trajectory using the simulation developed in this thesis (b). . . . . . . . . 355.8 Plot of wheel velocities versus time from both simulations. . . . . . . . . 35

vi

5.9 Scatter plot of the residuals from the wheel velocities. . . . . . . . . . . . 365.10 Plot of vehicle parameters using Baity’s simulation (a) and plot of vehicle

parameters using this simulation (b). . . . . . . . . . . . . . . . . . . . . 375.11 Plot of vehicle position from Baity’s simulation (a) wheel trajectory using

Baity’s simulation (b). . . . . . . . . . . . . . . . . . . . . . . . . . . . 385.12 Plot of vehicle position from the simulation from this paper (a) wheel

trajectory using the simulation developed in this paper (b). . . . . . . . . 385.13 Plot of wheel velocities verses time from both simulations. . . . . . . . . 395.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 vehicle’s

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

vehicle’s 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 vehicle’s 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 vehicle’s 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 axle’s

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. Jenning’s 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, Meadows’s 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 student’s 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 course’s lane boundaries and avoiding static obstacles. The Navigation Challenge tests

a robot’s 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 robot’s 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 it’s 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 Tech’s 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 vehicle’s

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 vehicle’s 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 vehicle’s 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 supportbar 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 vehicle’s heading, relative to magnetic

north. The digital compass is primarily used to determine the vehicle’s 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 camera’s 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 robot’s ”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 vehicle’s 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: Polaris’s 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 vehicle’s movement. The methodology of the kine-

matic model is to develop a model of the vehicle’s 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 ).

Rθ =

cosθ −sinθ 0

sinθ cosθ 0

0 0 1

(4.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.

ξCAF =

X

Y

θ

(4.2)

17

Taking the derivative of the position vector produces a velocity vector ( ˙ξCAF ) that defines

the motion of the front body within the global frame.

˙ξCAF =

X

Y

θ

(4.3)

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 Nourbakhsh’s 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 wheel’s 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 vehicle’s 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 vehicle’s 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 vehicle’s movement.

J1 =

−cosα1 +β1 sinα1 +β1 (−l)cosβ1

−cosα2 +β2 sinα2 +β2 (−l)cosβ2

(4.4)

The rotational velocity of each wheel (J2) can be expressed as the wheel’s angular

rotation (ϕ) multiplied by the radius (r) of each wheel.

J2(ϕ) =

rRF 0

0 rLF

ϕRF

ϕLF

(4.5)

19

To ensure there is pure rolling at the contact point, the wheel speeds (J2(ϕ)) are

constrained to produce motion along the direction of the wheel plane. The two terms

(Rθ , ˙ξCAF ) are used to map the motion of the front wheels to the global frame.

J1(Rθ )( ˙ξCAF ) = J2(ϕ) (4.6)

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.

C =

sinα1 +β1 cosα1 +β1 (l)sinβ1

sinα2 +β2 cosα2 +β2 (l)sinβ2

(4.7)

The sliding constraint is expressed in the global frame as

C(Rθ )( ˙ξCAF ) = 0 (4.8)

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

C

Rθ˙ξCAF =

J2(ϕ)

0

(4.9)

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 + link2e jθ2 + link3e jθ3 + link4e jθ4 = 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

( ~Vl f > ~Vr f ), then (R f ) is positive. Likewise, if ( ~Vl f < ~Vr f ), then (R f ) is negative.

R f =l f ( ~Vr f + ~Vl f )

( ~Vl f − ~Vr f )(4.12)

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 frontbody.

body (Vl f or Vr f ), the angular velocity of the front body (ω f ) can be determined.

ω f =~Vf

R f(4.13)

The distance (K f e jθK f ) from point (P) to the instant center (IC f ) can be determined by

inspection (Equation 4.14)(Figure 4.4).

K f e jθK f = R f + Pf e jθP f + LO f (4.14)

The value of point (P) (~VP) 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

~Vp j

~Vp k

=

K f cosθK f

K f sinθK f

0

×

0

0

ω f

(4.15)

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 (~Vf ) at the angle of P (θP).

~Vp = ~Vf e jθP 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) (~VP) can be written in terms of the angular velocity of the rear body (Equation

4.17).

~Vp = jωrKre jθKr (4.17)

A substitution for (Kre jθKr) 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 (Pre jωPr) 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).

Kre jθKr = −Rr +Pre jθPr (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 pointP.

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

~Vp j

~Vp k

=

−Rr +Pr cosθPr

Pr sinθPr

0

×

0

0

ωr

~Vp i

~Vp j

~Vp k

=

−ωrPr sinθPr

ωr (−Rr +Pr cosθPr)

0

(4.19)

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.

ωr =−~Vp i

Pr sinθPr(4.20)

Rr = −~Vp jωr

+ Pr cosθPr (4.21)

Now, using Equation 4.20 and Equation 4.21, the velocity of the left and right rear

wheels, denoted as ( ~Vlr) and ( ~Vrr) 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 ( ~Vl f = ~Vlr and ~Vr f = ~Vrr).

~Vlr = ωr(Rr− lr)

~Vrr = ω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

their values are listed in Table 5.1. Note that the front wheel speeds ( ˙ϕLF , ˙ϕRF ) 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

simlength Total time to run simulation 34 s ConstantCA f Vehicle origin in global space [0,0] m Constant

width f Distance between front wheel and center of axle 0.600 m Constantwidthr Distance between rear wheel and center of axle 0.521 m Constant

r f r Radius of the front right wheel 0.315 m Constantr f l Radius of the front left wheel 0.315 m Constantrrr Radius of the rear right wheel 0.315 m Constantrrl Radius of the rear left wheel 0.315 m Constant

LO f Distance between front axle and link 1 −0.236 m ConstantLOr Distance between rear axle and link 4 0.531 m Constantlink1 Distance from center of link to joint 0.669 m Constantlink2 Distance from center of link to joint 0.866 m Constantlink3 Distance from center of link to joint 0.669 m Constantlink4 Distance from center of link to joint 0.866 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 wheelsin 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 vehicle’s wheels in the global coordinateframe. 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 Baity’s work (0.5 m). Figure 5.5

shows an illustration of each vehicle’s linkage for the two simulations.

(a) (b)

Figure 5.5: Plot of vehicle parameters using Baity’s simulation (a) and plot of vehicleparameters 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 Baity’s simulation to plot

the vehicle’s position and wheel trajectory within the global coordinate frame. Figure

5.7 uses the simulation from this thesis to plot the vehicle’s position and wheel trajectory

within the global coordinate frame.

(a) (b)

Figure 5.6: Plot of vehicle position from Baity’s simulation (a) wheel trajectory usingBaity’s 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 Baity’s 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 Baity’s

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. Baity’s wheelvelocities are in black. Front and rear velocities from the simulation developed in thisthesis 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 vehicle’s 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 Baity’s simulation (a) and plot of vehicleparameters 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 Baity’s simulation to plot the vehicle’s

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 Baity’s 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 Baity’s simulation (a) wheel trajectory usingBaity’s 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. Baity’s wheelvelocities are in black. Front and rear velocities from the simulation developed in thisthesis 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 vehicle’s 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 vehicle’s 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 vehicle’s rear body, the navigation and control algorithms could be

developed to be oblivious of the vehicle’s 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 linkage’s 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 TwoRoad 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”, mastersthesis, (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:http://www.freepatentsonline.com/.

[6] Deng, Weiwen, et.al. ”Vehicle-Trailer Low-Speed Offtracking Control”, U.S. PatentUS20070152424, July 5, 2007. Available Online: http://www.freepatentsonline.com/.

[7] Elefteriadou, L., Glauz, W.D., Richard, K.R., Torbic, D.J., Harwood, D.W., ”ReviewOf Truck Characteristics as Factors in Road Design”, Transportation Research BoardOf 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 ComprisingSuch A Device”, U.S. Patent 4,429,894, Feb. 7, 1984. Available Online:http://www.freepatentsonline.com/.

44

[10] Grove, C.,D., ”Coupling Means For Dollys, Trailers, And The Like”, U.S. Patent2,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. Patent4,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 OfMachinery, Fourth Ed., (New York John Whiley & Sons, 1987).

[15] Meadows, Leon ”Tow Bar”, U.S. Patent 5,775,714, July 7, 1998. Available Online:http://www.freepatentsonline.com/.

[16] Propst, Robert,L., ”Steering Apparatus For A Trailer Vehicle”, U.S. Patent4,134,601, Jan. 16, 1979. Available Online: http://www.freepatentsonline.com/.

[17] Ramirez, R. A., ”Computer Vision Based Analysis of Broccoli for Application ina 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”, theMIT 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 forFour-Bar Linkage

Known:

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

Find:θ2

Solve for θ2 :

r1 + r2e jθ2 + r3e jθ3 + r4e jθ4 = 0 (A.1)

r1 + r2 cos(θ2)+ r3 cos(θ3)− r4 cos(θ4) = 0r2 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

r12 +2r1r2 cos(θ2)+2r1r3 cos(θ3)+2r2r3 cos(θ2)cos(θ3)+2r2r3 sin(θ2)sin(θ3)+ ...

r22 cos2 (θ2)+ r3

2 cos2 (θ3)+ r22 sin2 (θ2)+ r3

2 sin2 (θ3) = r42 cos2 (θ4)+ r4

2 sin2 (θ4)(A.3)

Use trig identitycos2 (θ)+ sin2 (θ) = 1

r12 + r2

2 + r32 +2r1r2 cos(θ2)+2r1r3 cos(θ3)+2r2r3 cos(θ2)cos(θ3)+ ...

2r2r3 sin(θ2)sin(θ3) = r42 (A.4)

Use trig identity:

cos(a−b) = cos(a)cos(b)+ sin(a)sin(b)

r12 + r2

2 + r32 +2r1r2 cos(θ2)+2r1r3 cos(θ3)+2r2r3 (cos(θ2−θ3)) = r4

2

(A.5)

Substitute to simplify:

U = r12 + r2

2 + r32 +2r1

2r32 cos(θ3)

V = 2r1r2 +2r2r3 cos(θ3)W = 2r2r3 sin(θ3)

U +(V +W )cos(θ2)+W sin(θ2) = r42

(A.6)

Use trig identity, Tangent Half-Angle Formula [14]

t = tan(

θ22

), cos(θ2) =

1−tan2(

θ22

)1−tan2

(θ22

) , sin(θ2) =2tan

(θ22

)1+tan2

(θ22

)

U +(V +W )(

1− t2

1+ t2

)+W

(2t

1+ t2

)= r4

2

47

U(1+ t2)+(V +W )(1− t2)+W (2t) = r42(1+ t2)

U(1+ t2)+(V +W )(1− t2)+W (2t)− r42(1+ t2) = 0

(U−V −W − r42)t2 +2V (t)+(U +V +W − r4

2) = 0 (A.7)

Substitute to simplify:

A = (U−V −W − r42), B = 2V (t), C = (U +V +W − r4

2)

At2 +Bt +C = 0

A(

tan2(

θ2

2

))+B

(tan

(θ2

2

))+C = 0

tan(

θ2

2

)=−B±

√B2−4(A)(C)2(A)

θ2 = 2(

arctan(−B±

√B2−4(A)(C)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)

m2 =y2

x2, m4 =

y4

x4

Point Slope Formula:

y− y0 = m(x− x0) (B.1)

49

y2− y0 = m2(x2− x0)y4− y0 = m4(x4− x0)

m2x0− y0 = m2x2− y2

m4x0− y0 = m4x4− y4 (B.2)

Rearrange in matrix form:[x0y0

]·[

m2 −1m4 −1

]=

[m2x2 − y2m4x4 − y4

]

[x0y0

]=

[m2 −1m4 −1

]−1

·[

m2x2 − y2m4x4 − y4

](B.3)

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, metersrfl = .3150; %Radius of the front left wheel, metersrrr = .3150; %Radius of the rear right wheel, metersrrl = .3150; %Radius of the rear left wheel, meterstheta = 0*(pi/180); %intial angle of the vehicle in global space, degreesgamma = 0*(pi/180); %initial angle between the front section robot frame,

%degrees and the rear sectionCA_f = [0;0]; %vehicle origin in global spacewidth_f = .6004; %distance between the front wheel and the center of axle

%(width/2), meterslength_f = .2362; %distance between center of axle and front body frame% (length/2), meterswidth_r = .5216; %distance between rear wheel and center of axle% (width/2), meterslength_r = .3429; % distance between center axle and rear body frame% (length/2), meterswheel_speed_fl = 1; %rotational speed of the left wheel, radians/sec

51

wheel_speed_fr = 1; %rotational speed of the right wheel, radians/secLO_f = -.2362; %negative Y distance between the front axle and the% joints of link1, metersLO_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, meterslink2 = .8661; %measured as lenght of link, meterslink4 = .8661; %measured as length of link, meters

% ++++++ Simulation Model Parameters ++++++delta_t = 1; %seconds%sim_length = 59; %number of seconds to run the simsim_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 displayPhi_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 analysisCA_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 countsii = ii + delta_t;kk = kk + 1;

if ii <= 0wheel_speed_fl = 1; %rotational speed of the left wheel, rad/secwheel_speed_fr = 1; %rotational speed of the right wheel, rad/sec

elseif ii <= 25wheel_speed_fl = 1;wheel_speed_fr = .5;

53

elsewheel_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 axisrot_z = [cos(theta),-sin(theta);sin(theta),cos(theta)];%Translatetranslate = [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 ontitle(’Front and Rear Wheel Travel’,’FontSize’,14)ylabel(’Global Y Distance, Meters’,’FontSize’,14)

54

xlabel(’Global X Distance, Meters’,’FontSize’,14)axis equalgrid 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%4 Bar Simulation of a 2 Body Vehicle%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*eˆjtheta2 + link3*eˆjtheta3 - link4*eˆjtheta4 = 0%=========================================================================

%Combine elements of the kinematic 4Bar equation to clean up equationsA1 = link1ˆ2 + 2*link1*link3*cos(theta3) + link2ˆ2 + link3ˆ2;B1 = 2*link1*link2 + 2*link2*link3*cos(theta3);C1 = 2*link2*link3*sin(theta3);

55

%Combine again to clean up equationA2 = A1-B1-link4ˆ2;B2 = 2*C1;C2 = A1+B1-link4ˆ2;

%Use tan half angle identitiy --> t = tan(theta2/2)%This gives you a quadratic funtion (A2tˆ2 + B2t + C2) to solve theta2theta2_1 = 2 * atan((-B2 + sqrt(B2ˆ2 - 4*A2*C2))/(2*A2));theta2_2 = 2 * atan((-B2 - sqrt(B2ˆ2 - 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 theta4theta4_1 = atan2((link2*sin(theta2_1) + link3*sin(theta3)),...

(link1 + link2*cos(theta2_1) + link3*cos(theta3)));theta4_2 = atan2((link2*sin(theta2_2) + link3*sin(theta3)),...

(link1 + link2*cos(theta2_2) + link3*cos(theta3)));theta4_3 = atan2((link2*sin(theta2_3) + link3*sin(theta3)),...

(link1 + link2*cos(theta2_3) + link3*cos(theta3)));theta4_4 = atan2((link2*sin(theta2_4) + link3*sin(theta3)),...

(link1 + link2*cos(theta2_4) + link3*cos(theta3)));

%Evaluate each term of theta2 and theta4 to find which combination will%equate to zerotheta2 = [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];endend

%theta_check;

56

% Determine which element in the matrix theta_check, is the value for% thata2 and theta4theta2 = theta_check(1,:);theta2 = theta_check(find(theta2<0)); %use the ’find’ command to% determin values below zerotheta2 = 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 link4link2_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 link4y2 = 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 link1LO_f_1 = [0;0];%Find x,y coordinates of the center of link3link3_2 = [(link3/2)*cos(theta3);-(link3/2)*sin(theta3)];%Find the center of link3LO_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 axleCA_r = [LO_r_1(1,1) + LO_r*sin(theta3);LO_r_1(2,1) - LO_r*cos(theta3)];%Find the length of vector PfPf = sqrt((P(2,1) - LO_f_1(2,1))ˆ2 + (P(1,1) - LO_f_1(1,1))ˆ2);%Find the angle of vector Pfphi_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 PrPr = sqrt((P(2,1) - LO_r_1(2,1))ˆ2 + (P(1,1) - LO_r_1(1,1))ˆ2);%Find the angle of vector Prphi_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_Dlink_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 4Barfigure(2)set(gcf, ’Toolbar’, ’none’);set(gcf, ’Units’, ’inches’);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%4 Bar Simulation of a 2 Body Vehicle%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 axisrot_z = [cos(theta),-sin(theta),;sin(theta),cos(theta)];%Translatetranslate = [0;LO_f];%Translate and rotate link point into global coordinate framelink2_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 wheelsf_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 wheelsr_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, ’Toolbar’, ’none’);set(gcf, ’Units’, ’inches’);set(gcf, ’Position’, [4.063 3 4 4]);hold onplot([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’,...[link2_2(1,1),link4_2(1,1)],[link2_2(2,1),link4_2(2,1)],’-k’,...[link4_2(1,1),link4_1(1,1)],[link4_2(2,1),link4_1(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 equalgrid onxlabel(’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%4 Bar Simulation of a 2 Body Vehicle%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 axlealpha1 = -pi/2;alpha2 = pi/2;beta1 = pi;beta2 = 0;%Postion Velocity MatrixRadi = [rfr 0; 0 rfl];Wheel_speed=[wheel_speed_fr;wheel_speed_fl];Wheel_dot_f=[Radi*Wheel_speed;0];

62

%Rolling ConstraintsJ1f_1 = [-cos(alpha1+beta1) sin(alpha1+beta1) -width_f*cos(beta1)];J1f_2 = [-cos(alpha2+beta2) sin(alpha2+beta2) -width_f*cos(beta2)];%Sliding ConstraintsC1f1 = [sin(alpha1+beta1) cos(alpha1+beta1) width_f*sin(beta1)];C1f2 = [sin(alpha2+beta2) cos(alpha2+beta2) width_f*sin(beta2)];%Constraint matrixC=inv([J1f_1;J1f_2;C1f1]);%Rotation Matrix, rotation about z axis to get into global coordinate framerot_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%infinateif 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 aheadgamma_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_PrP_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 vehicleelseif 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_PrP_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)];

%Add the Link Offset LO_r to the vector Pr in the back body frameP_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/2Vrr_wheel = -P_dot_r;Vrl_wheel = -P_dot_r;

else%Calculate the rotation velocity of the Instant Center of the rear bodyOmega_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 CenterR_r = (P_dot_r(2,1)/Omega_IC_r) - P_rear(1,1);

%Calculate the velocities of the right and left rear wheelsVrr_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 velocitiesgamma_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 velocitiesR_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;

elseOmega_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 frameP_rear = [0;LO_r] + [Pr*cos(phi_Pr);Pr*sin(phi_Pr)];

%Calculate the P_dot according to the rear bodyP_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;P_dot_r = [P_dot_mag*cos(P_dot_angle);P_dot_mag*sin(P_dot_angle)];

if P_dot_angle == pi/2Vrr_wheel = P_dot_mag;Vrl_wheel = P_dot_mag;

elseif P_dot_angle == -pi/2Vrr_wheel = -P_dot_mag;Vrl_wheel = -P_dot_mag;

else%Calculate the rotation velocity of the Instant Center of the rear bodyOmega_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 CenterR_r = (P_dot_r(2,1)/Omega_IC_r) - P_rear(1,1);

%Calculate the velocities of the right and left rear wheelsVrr_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;

%%%%Peter’s 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 velocitiesgamma_dot = (Vrr_wheel - Vrl_wheel)/(2*width_r);

end

66