Principle Introduction and Theoretical Support Analysis: Ballistics
This page introduces the theory behind the ballistics calculation for determining pitch and yaw used in aim_turret.py. See function calc_pitch_yaw starting at line 186.
Our robot conditions and assumptions:
- Single IMU on chassis
- Odometry data (linear and angular position, twist and acceleration) of our own robot is being tracked in the static frame ('odom' frame)
- Linear position, velocity and acceleration of the primary target to be aimed at is being tracked in the static frame ('odom' frame)
- Friction and air resistance are negligible
- turret base frame (named "base_turret_home") is the frame of reference from the intersection between the pitch and yaw axes on the robot's turret
- acceleration due to gravity is a constant -9.8 in the z-direction
- projectile launch velocity is always constant every shot (defined by
self.BULLET_V) - Using ROS coordinates: +x is straight ahead, +y is left, +z is up
The algorithm first converts the measurements in the static frame into measurements in the turret base frame and then solves for pitch and yaw by solving the parametric equation for time of projectile flight.
Since the measurements are not continuous, they were obtained some time in the past. Since our ballistics calculation assumes that the projectile is being launched at time now, we use our constant acceleration model for odometry and target tracking to forecast ahead to predict their state at the current time. (The +0.03 on line 128 is a fudge factor added in to mitigate other sources of lag, have not investigated, is an area for further improvement).
Relevant lines: 128-139
Use the get_predicted_primary_target_state function (line 128) and _predict_const2deriv_state function (line 138)
Our odometry measurements are of the chassis center (named "base_center") and not of the turret base frame. To get measurements in the turret base frame, we must first locate turret base frame in the static frame.
Most importantly, we must get the rotational transformation of the turret base frame relative to the static frame. Since the robot chassis can get tilted on slopes or from mechanical inconsistencies, the yaw and pitch planes are not always parallel and perpendicular to the world horizontal respectively. So, to get the correct pitch and yaw bearing, the measurements must be converted into the turret base frame first
Relevant lines: 134-167
a. Obtain the static transformation property from chassis center to turret base. This comprises both the translation and quaternion, q1, defining the relative rotation from chassis center to turret base. However, the latter is the default (0, 0, 0, 1) since the turret center is assumed to be mechanically fixed horizontal to the chassis.
b. Obtain the quaternion, q2, defining the rotation of the chassis relative to the static frame.
c. Invert q1 and q2 for ease of frame transforms. Since static frame --q2--q1--> base turret frame, to convert a vector in the static frame to the base turret frame, we apply q1-inverse then q2-inverse to the vector.
Relevant lines: 197-218
a. Obtain the target's position, velocity and acceleration relative to the base turret frame by subtracting our own position, velocities and accelerations from the target's and then multiplying them by q1-inverse then q2-inverse. Note: for production we currently have target acceleration set to 0 due to there being too much noise for acceleration in our target tracking Kalman Filters.
b. Obtain acceleration due to gravity in the turret base frame by multiplying (0, 0, -9.8) by q1-inverse then q2-inverse
Now that the measurements are in the base turret frame, we derive the ballistics solution.
Let t be time, L be barrel length, V be projectile launch velocity.
Let be the target position, velocity and acceleration in base turret frame
Let be pitch,
be yaw. 0 pitch is horizontal with positive direction going up and 0 yaw is straight ahead with ccw direction being positive.
Let (a, b, c) be acceleration due to gravity in base turret frame
Assume the projectile leaves the barrel at t=0. Equate the path of the target to the path of the projectile:
Rearranging terms (move a, b, c to left and factorize L+Vt):
Let ,
,
. Then, summing the squares of the first 2 lines of the simultaneous equations, we get:
Because . Similarly, if we add this to the square of the 3rd line of the simultaneous equations, we get:
Now we only have an equation of 1 variable, t. Expanding out and
and then moving all the terms to one side, we get
A fourth order polynomial that can be solved efficiently.
Once we have solved for t, we can solve for pitch and yaw:
Pitch:
Yaw: divide the second equation by the first equation and we get
Relevant lines: 221-258
We use numpy's roots function to solve the 4th order polynomial for t and then use the appropriate trigonometry functions to do the final calculation for pitch and yaw.
a. Performance measurement
b. If required, move this calculation onto the controls side for faster and more consistent response.
c. Further optimizations: if performance measurements determine that the calculation is not fast enough, we could switch to less precise numerical methods such as newton's method to solve for the fourth order polynomial of t
d. Account for variable projectile launch velocities.
e. Simplify calculations if we change to a IMU on turret setup
f. Investigate sources of lag so that we can remove the +0.03 fudge factor