Controlled Steering Relating to Kinematic models of 2D steering and turning Prof. R.G. Longoria Spring 2015
How do you steer a differential vehicle to reach a desired location/orientation? How do you determine where to go? How do you incorporate your initial condition? How do you implement actions?
The Roaming Program for DaNI In lab, we ll study the Roaming example, which illustrates how the ultrasonic (PING) sensor can be used to detect an obstacle free path a desired heading angle. This information comes from the Calculate Driving Direction subvi (see below). Within this VI is an algorithm for roaming.
Calculate Driving Direction Study this subvi, which uses a Vector Field Histogram subvi that is part of the LV Robotics Module. This VI makes use of the PING sensor distance data and the servo angle positions. Decisions are made based on obstacle/gap distances. Next few slides explain some of the VFH subvi
Key outputs used
Panic range parameters need to be defined. Largest gap data includes both the angle to the gap and size Nearest obstacle data can be used for driving decisions
Distances to obstacles may determine that you need to drive away, so a case structure is used to specify one of two driving routines. This case is for driving away.
If not in a panic range the Roaming algorithm will drive towards a gap using the angle to the gap as an instantaneous desired heading angle. Note that the body-fixed velocity variable names in LabVIEW are not conventional. Flip x_dot and y_dot here. These relations specify a desired heading angle as the angle to the gap most likely to allow obstacle free navigation. These are purely heuristic. See Borenstein and Koren*, who describe a similar way of specifying steering frame velocity based on angle to a gap. Angle to gap Gap size *Borenstein and Koren* 1991 article posted on course log.
Be very sure to understand that the relations for the desired steering frame velocity (the three body-fixed velocities) are not based on a model. In summary: 1. The sensing provides a heading angle to a gap: 2. The relations: 2 vxd = vx,max 1 π ψ d ψ d 2 ωzd = Ωz,max π ψ d specify desired (body-fixed) steering frame velocities 3. Need to generate desired differential steering control commands for the two motors to achieve the desired heading angle.
In the Roaming program, the steering frame velocity setpoints are sent to the LVRM Apply Steering Frame Velocity to Motors subvi:
This is the block diagram for the Apply Steering Frame Velocity to Motors subvi used in the Roaming program. Not needed Not needed vx v y ω z desired desired steering frame velocity motor speed commands Not needed ω1 ω 2 desired This VI is a highly generalized routine. Our goal is build a simpler one from scratch using the 2D differential steering kinematic model.
Recall the body-fixed kinematic velocities ( steering frame velocity ) qɺ 1 1 1 2 w 2 w vx 2 Rw ( ω1 + ω2) l2rw l2rw 1 v ω y l2ω z B B ω2 ω z R w ( ω1 ω 2 ) Rw R w B = = = R B R B We need ω ω 2 1 1 2 Rw 2 Rw l R l R = B B Rw Rw B B 1 2 w 2 w 1 vx v y ω z But this is a non-square matrix. Need Moore Penrose pseudo-inverse Easy numerically (next slide) to find the motor velocity commands.
Easy to implement a numerical pseudo inverse if you wanted to use the more accurate model of the vehicle where the CG location is a little off the axle. L = 8/39.37; % wheel base L1 = 1/39.37; L2 = L-L1; B = 14.5/39.37; % rear axle track width Rw = 2/39.37; % wheel radius A = [Rw/2 Rw/2;L2*Rw/B -L2*Rw/B;Rw/B -Rw/B] pinv(a) ans = 19.6850 0.6248 3.5139 19.6850-0.6248-3.5139
For the simplified case where CG is on axle, we can find analytical relations for the differential steering wheel velocities by removing the y axis and, 1 1 1 2 Rw 2 Rw ω1 vx 1 Rw 1 2RwB vx Rw R w ω = 2 ω = z 1 Rw 1 2RwB ω z B B So, what is this telling us? If we want to generate these particular body-fixed (steering) frame velocities, then the angular velocities of the wheels can be specified using inverse kinematic relations. This is an open loop (or feedforward) type of control. By using the sensing to specify desired body-fixed steering frame velocities, it is possible to specify control commands to steer the vehicle to a heading angle. The control loop continuously updates the heading angle estimate, so in a sense it can tend toward zero. Study what this means in the steering frame velocity commands or setpoints.
ω1 ω 2 This motor drive subvi from the LVRM can be replaced by your own drivetrain control VI(s). Replace this routine with code derived from the kinematic model