UNICYCLE PHYSICS MATHS
======================
CONTENTS
0. INTRODUCTION AND LICENSE
1. OVERVIEW
2. RIDER MODEL OVERVIEW
3. VARIABLES THAT WE MAY USE
4. PHYSICAL COEFFICIENTS AND CONSTANTS
5. CALCULATIONS OF MOTION
6. TEMPORARY "FULL" EQUATIONS
7. STANDARD SIMPLIFIED EQUATIONS
8. A PRACTICAL NOTE
9. ADDING FRICTIONAL EFFECTS
10. GENERAL SYSTEM CONTROL AND FEEDBACK
11. EVOLUTION OF STATE
12. THE HUMAN SYSTEM CAPABILITIES
13. THE HUMAN RIDER MOTION CONTROL MODEL
14. SOME AREAS OF POSSIBLE CONCERN OR CHANGE
15. THE GRAPHICS
16. DATA FLOW AND STRUCTURES IN THE SVG SCRIPT
17. COMPARISON WITH SOME PUBLISHED RESEARCH
18. COMPARISON WITH UNICYCLING REALITY
19. BROWSER CONSIDERATIONS
20. USEFUL(?) LINKS
0. INTRODUCTION AND LICENSE
The documentation, svg images and script were created in 2022 by:
John Evershed of Project Computing Pty Limited, Australia.
The documentation, svg images and script are licensed under:
https://creativecommons.org/licenses/by-nc-nd/3.0/.
[ie "Attribution-NonCommercial-NoDerivs"].
Links to the works are at:
https://flight.projectcomputing.com/v/
For contact details, see:
https://projectcomputing.com/
This is documentation and math preparation for 2D scripted svg simulation.
The simulation is done using ONLY SVG with embedded javascript,
without using and external javascript libraries.
The SVG is relatively small (~50KB) and operates locally on the browser,
without making any connection to any servers.
We don't use (eg) MATLAB.
So we avoid cost and 3rd party code package dependencies.
We represent the motion of a unicycle subject to rider torque input,
using a kinematic model.
We also simulate a sensor system, and a human controller (rider).
The web user can vary parameters (such as target pedal RPM),
and even have a pretend "chat" with the rider.
Various motion data (such as RPM and pitch) are shown on a dashboard.
1. OVERVIEW
We model the unicycle wheel connected at an axle to a rigid frame+rider,
with torque applied at the axle according to the whims of the rider.
The "rider" is a tuned feedback control system aiming for steady forward speed.
This is a 2D inverted pendulum type of thing (ie pitch, but no roll or yaw).
It has 2 main variables x (distance of axle along road) and phi (angle of pitch),
and the time derivatives of each (vx, vphi) as extra state variables.
Although the graphics will show moving legs, the resultant slight oscillation
of the pitch angle is NOT simulated in the system model.
We will derive standard linearised equations of motion vz = A.z + u.B
where z = [x,vx,phi,vphi] is a state vector, and u is a scalar (torque).
A is the system matrix, B is the input matrix.
In practice, both the horizontal acceleration, ax,
and also the pitch angle acceleration (aphi, clockwise from 0 being vertical),
mainly depend on phi (via A, due to gravity) and on rider input torque (via B).
Our simulation uses numerical values corresponding to a real unicycle and rider,
including moments of inertia. (body twisting and arm waving is not modelled).
The simulation is slightly different depending on pitch angle phi.
Near the balance point, we use the linearised equations (A and B).
But if (say) pitch is less than -15 degrees or more than 20 degrees,
we assume the torque stops and the frame+rider falls.
When the pitch gets near -90 degrees or +90 degrees,
the pitch is set at that angle and vx rapidly becomes zero, ending the simulation.
The graphic of the unicycle+rider is updated at the frame rate (up to 50fps),
according to the motion equations. ie we integrate with dt, dx, dphi, dvx, dvphi.
We assume the rider approximately "observes" the computed full state at each dt,
and uses this and the target and some recent state history,
and "pre-tuned" feedback, for control (by setting a torque input, Tin).
the goal is relative pitch stability and slowish fairly steady forward speed.
The torque is regularly adjusted ("rider" control) by state feedback and prediction.
These control adjustments happen at a slower rate (eg every 2nd or 5th frame).
The desired state is phi=0 and vx is fairly constant at about 0 to 8 m/s.
Observed error in phi (along with vphi) has maximum feedback priority,
vx error (from 1m/s) is low priority unless vx is negative or near maximum.
btw max vx is about 3.87m/s at 100rpm or about 14kph. (wheel radius 0.37m).
The torque is subject to human limitations with a squared sinusoidal value,
having a maximum peak value (and less absolute max value if vx is negative).
The implied wheel rotational speed is subject to an RPM limit of about 200.
We will model rider ability (and road "noise") by adding a random error
to the torque feedback calculation or adding an error to the observed
values of the state variable or by supplying control feedback less frequently.
We could also allow the time to be slowed, eg by doing the motion calculations
every 2nd frame, but duplicating or interpolating the frame in between.
2. RIDER MODEL OVERVIEW
At "start", the system will be given a slightly positive (forward) pitch.
The rider system has a goal of staying upright and maintaining a speed
of approximately 2m/s with some tolerance for speed variation.
The forward pitch roughly means at t=0, phi=0.02.
depending on the "launch" method, we could set vphi=0,
or perhaps calculate it, assuming wheel fixed as frame+rider tilts via gravity..
vphi = sqrt(2*M*g*R/(M*R^2+J)*(1-cos(phi))) //from energy considerations
so vphi = sqrt(7.51*(1-cos(phi))) //as M=77kg, R=0.85m, J=18.7kg_m^2
for small phi, vphi = 2*phi roughly
if phi = 0.05236 radians, vphi = 0.101rad/s //ie at 3 degrees
With the assumed speed target, given the assumed unicycle wheel parameters,
target speed will be equivalent to an rpm (cadence) of about 50 per minute.
The unicycle+rider system will move according to the calculated system matrix.
The instantaneous rider torque might be Tin * sin2(pi/2 + x/r).
(btw the pi/2 offset reflects starting at t=0, x=0 with a pedal at "3 o'clock").
The cadence is 60*vx/(2*pi*r).
A 70kg rider with all weight on a horizontal pedal produces a torque
of 70*9.8*0.125 = 85.75Nm (if pedal crank length, l=125mm).
This corresponds to a Tin of 42.375Nm (as Tin is half the peak torque).
Note that Tin is the rider torque input AVERAGED over a pedal cycle.
At a pedal angle of 0, the torque is near 0, and at pi/2 the torque is near 2*Tin.
At intervals, the Tin value will be changed by the rider system feedback.
At roughly every 0.02s (ie frame rate of 50fps) the unicycle motion data is updated.
At every 0.1s, the rider is assumed to observe the motion data.
The rider may have access to the following feedback data, for Tin adjustment:
1. current rpm = 60*vtheta/(2*pi), where = vtheta = vx/r.
2. current Tin setting
3. qphi as observed tilt angle (delayed phi + small random error)
4. qvphi as observed rate of tilt (delayed vphi + small random error)
5. qaphi as observed acceleration of tilt (delayed aphi + small random error)
6. time weighted average of qphi (ie memory)
7. time weighted average of Tin (ie memory)
In the implementation, only some of this "sensor feedback" is used.
The feedback data will be fed into a simple control model which has some
hand tuned parameters.
One option is that a couple of these parameters may also be further tuned
in simulated riding - ie there could be a cost function and a reward
function for a ride from start to "fall", with deltas fed back to the
parameters according to their performance benefits at each ride end,
similar to some artificial neural net learning.
The human controller model is quite separate from the wheel+frame+rider
system model for mechanical motion subject to geometry, gravity and rider torque.
So it should be easy to fiddle with the human controller.
3. VARIABLES THAT WE MAY USE
t = time (seconds from start)
dt = time between frames
phi = tilt angle (radians)
vphi = angular velocity of phi (ie time derivative of phi)
aphi = angular acceleration of phi
x = horizontal position relative to starting position
vx = horizontal linear velocity
ax = horizontal acceleration
z = [x,vx,phi,vphi] is state variable
T = torque at the axle (time varying input from rider)
Tin = average value of rider's current sinusoidal squared torque setting
Tmax = maximum (positive) T, if needed
Tmin = minimum (negative) T, if needed
Tinmax = maximum (positive) Tin value assumption = Tmax/2
Tinmin = minimum (negative) Tin value assumption = Tmin/2
Tdrag = small frictional effect (sometimes used)
Tpedal = T, but only when considering Tdrag
theta = wheel rotation angle (little use, is pi/2 radians at t=0)
gamma = road inclination (mostly assumed to be zero)
l = pedal crank length (little use) = 0.125m
rpm = pedalling cadence (tied to vx)
x_ = temp saved x prior to most recent motion state update
vx_ = temp saved vx prior to most recent motion state update
phi_ = temp saved phi prior to most recent motion state update
vphi_ = temp saved vphi prior to most recent motion state update
phifall = 0.157 //9 degrees
phifallback = -0.122 //-7 degrees
phislide = 1.48 //85 degrees (negative or positive)
xfuzz = sd of random x observation error = 0.05?
vxfuzz = sd of random xv observation error = 0.1?
rpmfuzz = sd of random rpm observation error = 2? (tied to vx)
phifuzz = sd of random phi observation error = 0.005?
vphifuzz = sd of random vphi observation error = 0.01?
thetafuzz = sd of random theta observation error = 0.1?
tinfuzz = sd of random Tin application error = 0.1?
qphi = observed phi (by rider, with fuzzy error and slight delay)
qvphi = observed vphi
xaphi = observed aphi (unused)
qx = observed x (unused)
qvx = observed vx
qavx = observed ax (unused)
qrpm = observed rpm
qtheta = observed crank angle
vxtarget = desired forward speed (eg 2m/s)
vxtarget = desired cadence (eg 50rpm)
phitarget = desired pitch angle (around zero)
evx = qvx-vxtarget (forward speed error)
ephi = qphi-phitarget (pitch error)
hevx = weighted history of evx (unused)
hephi = weighted history of ephi up to ephi(t-dt)
pephi = forward prediction of ephi (eg ephi+dt*qvphi)
dephi = latest change in ephi
whephi = weight for adding latest ephi to hephi
//hephi = (ephi(t)+hephi*whephi)/(1+whephi)
e1 = adjusted observed phi error
e2 = possibly adjusted vx error
e1min, e1max, e1negmax, e1delta, e2min
(used to decide whether e1 or e2 predominates in deciding tin)
kephi = tunable weight for ephi
khephi = tunable relative weight for hephi
kpephi = tunable relative weight for pephi
kdephi = tunable relative weight for dephi
ke1 = multiplier for e1 (or modified e1) to yield Tin
Ke2 = multiplier for e2 (unused)
k1 = coefficient of T in linearised equation for ax
k2 = coefficient of phi in linearised equation for ax
k3,k4 reserved for friction and road inclination effects for ax equation
j1 = coefficient of T in linearised equation for aphi
j2 = coefficient of phi in linearised equation for aphi
j3,j4 reserved for friction and road inclination effects for aphi equation
u sometimes used in matrix form (for T), ie in vz = Az + Bu
A used for matrix form of equations of motion (nb also used differently below)
B used for matrix form of equations of motion
A,B,C,D,E,F using for convenience in deriving equations of motion
Q maybe used as a generic for the u input forcing term
q and qv maybe used as a generic for x and xv or phi and vphi
KE is kinetic energy
Kw is KE of wheel, Kf is KE of frame+rider
PE is potential energy
Pw is PE of wheel, Pf is PE of frame+rider
d1,d2,d3 may be used for eigenvalues
v1,v2,v3 may be used for eigenvectors
4. PHYSICAL COEFFICIENTS AND CONSTANTS
g = gravity acceleration (9.8m/s^2)
wheel specs:
m = mass (3kg)
r = radius from axle to rolling surface (ie road) (0.37m)
I = moment of inertia of wheel COG about axle (kg.m2)
frame+rider specs:
M = mass (77kg)
R = radius from axle to COG of frame+rider (0.85m)
J = moment of inertia of frame+rider COG about axle (kg.m2)
I and J approximations:
I = (2/3)*3*0.33^2 = 0.22kg_m^2
J = (1/12)*77*((2*0.85)^2+3*0.10^2) = 18.7kg_m^2 //bar radius 10cm
l = pedal crank length = 0.125m
Tmax = 100Nm //peak
Tmin = -50Nm //peak
Tinmax = 50Nm //for average over pedal cycle
Tinmin = -25Nm //for average over pedal cycle
5. CALCULATIONS OF MOTION
End result after simplification will be a pair of equations for ax and aphi..
ax = k1*T + k2*phi
aphi = j1*T + j2*phi
To get these equations, We do Euler-Lagrangian calculations etc from scratch...
x and phi define the position.
[btw, theta is reserved for wheel angular position, gamma for road incline]
unicycle is really on an inclined plane which may slowly tilt slightly.
x axis is always in direction of usual forward travel (to right of page).
the whole xy axis may tilt by gamma from the horizontal (gamma>0 = uphill).
visualisation will have a fixed unicycle and rolling road.
initial implementation will use gamma=0.
at t=0, x=0
phi measures the frame+rider pitch angle.
phi = 0 if axle to frame+rider COG is in line with gravity
so if phi>0, the frame is tilting clockwise (forward to the right)
NB phi is NOT measured relative to y axis (we do not need y).
Torque is applied at the wheel axle.
Torque approximated by sin squared crank angle (sin2(theta)).
a constant very small frictional torque is assumed when rolling.
So a torque of T has a force F to the road of T/r,
and applies equally (Newton's 3rd law) to wheel and frame+rider.
the chosen state values are:
x, vx (the x value and velocity of the axle);
phi, vphi (the pitch angle relative to gravity "up" vector, and its velocity).
ax and aphi are d/dt(vx) and d/dt(vphi) where needed.
KE is kinetic energy = Kw + Kf (ie wheel + frame energy)
PE is changeable potential energy = Pw + Pf (ie wheel + frame energy)
we ignore any potential energy which never changes, of course.
so actually PE = Pf (unless wheel goes uphill)
We will use Lagrange for mechanical systems (KE=kinetic, PE=potential),
we can write (1) - (2) + (3) = (4) for each q (ie x or phi)
(1) is d/dt(dKE/dvq)
(2) is dKE/dq
(3) is dPE/dq
(4) is the effect of some external force in the q direction, say Qq
(4) involves the non conservative force T/r from the axle
it increases the x by dx proportional to (T/r)
but it *decreases* the phi by dphi proportional to (T/r)*r
so Qx = T/r, Qphi = -T
to do the LHS, we need to write down KE and PE..
PE = Mg*R*cos(phi)
KE1 = 0.5*m*vx^2 //wheel linear
kE2 = 0.5*I*(vx/r)^2 //wheel rotational
kE3 = 0.5*J*vphi^2 //frame+rider rotational about COG of M
now KE4 is based on vector velocity of COG of M (say v)
v is split into 2 components at right angles
v1 = vx + R*cos(phi)*vphi
v2 = R*sin(phi)*vphi
so v1^2 = vx^2 + 2*vx*R*cos(phi)*vphi + R^2*cos2(phi)*vphi^2
and v2^2 = R^2*sin2(phi)*vphi^2
then v^2 = vx^2 + 2*vx*R*cos(phi)*vphi + R^2*vphi^2
KE4 = 0.5*M*vx^2 + MR*vx*cos(phi)*vphi + 0.5*M*R^2*vphi^2
first look at the LHS wrt x..
dKE/dvx = m*vx + (I/r^2)*vx + M*vx + MR*cos(phi)*vphi
so x(1): d/dt = m*ax + (I/r^2)*ax + M*ax + MR*cos(phi)*aphi - MR*sin(phi)*vphi^2
x(2): dKE/dx = 0
x(3): dPE/dx = 0
so T/r = m*ax + (I/r^2)*ax + M*ax + MR*cos(phi)*aphi - MR*sin(phi)*vphi^2
next look at the LHS wrt phi..
dKE/dvphi = J*vphi + M*R^2*vphi + MR*vx*cos(phi)
so d/dt = J*aphi + M*R^2*aphi + MR*d/dt[vx*cos(phi)]
phi(1): d/dt(dKE/dvph) = J*aphi + M*R^2*aphi + MR*cos(phi)*ax - MR*sin(phi)*vx*vphi
phi(2): dKE/dphi = -MR*sin(phi)*vx*vphi
phi(3): dPE/dphi = -MR*g*sin(phi)
so -T = J*aphi + M*R^2*aphi + MR*cos(phi)*ax - MR*g*sin(phi)
Therefore..
T/r = m*ax + (I/r^2)*ax + M*ax + MR*cos(phi)*aphi - MR*sin(phi)*vphi^2
-T = J*aphi + M*R^2*aphi + MR*cos(phi)*ax - MR*g*sin(phi)
the system has an assumed balance point at vx=0, vphi=0 and phi=0
so we can use small angle trigonometric assumption about such values.
note that x is a "free" variable.
6. TEMPORARY "FULL" EQUATIONS
please jump ahead to section 7 headed as "STANDARD SIMPLIFIED EQUATIONS",
below, to help preserve your sanity.
we might like the non-linearised form for checking, or for simulating
the more exact reaction to the calculated dT derived from the linearised
equations in the control feedback loop (to test the non-linearity),
but it will still not be perfect due to the sampling interval (dt).
we have our 2 non-linear equations (with annoying sin and cos phi and vphi^2)..
T/r = A*ax + [B*cos(phi)]*aphi + [-B*sin(phi)]*vphi^2
-T = [B*cos(phi)]*ax + E*aphi + [F*sin(phi)]
we will borrow the A,B etc from the FULLY simplified (no slope or friction)
form in section below..
ie let A=[m+M+I/r^2], B=[MR], E=[J+MR^2], F=[-MR*g]
so from 2nd eqn we derive the following by making ax, and then aphi the subject..
ax = [-1/(B*cos(phi))] * [T + E*aphi + [F*sin(phi)]]
aphi = [-1/E] * [T + [B*cos(phi)]*ax + [F*sin(phi)]]
now substitute each of these into the 1st non-linear eqn,
and make 2 new equations for ax and aphi so neither have both ax AND aphi..
Step 1:
T/r = A*ax + [B*cos(phi)]*aphi + [-B*sin(phi)]*vphi^2
and ax = [-1/(B*cos(phi))] * [T + E*aphi + [F*sin(phi)]] ...
so T/r = [-A/(B*cos(phi))] * [T + E*aphi + [F*sin(phi)]]
+ [B*cos(phi)]*aphi + [-B*sin(phi)]*vphi^2
T/r = [-A/(B*cos(phi))] * T
+ [-A/(B*cos(phi))] * E*aphi
+ [-A/(B*cos(phi))] * [F*sin(phi)]
+ [B*cos(phi)]*aphi
+ [-B*sin(phi)]*vphi^2
[1/r + A/Bcos(phi)] * T = [B*cos(phi) -AE/(B*cos(phi))] * aphi
+ [-A/(B*cos(phi))] * [F*sin(phi)]
+ [-B*sin(phi)]*vphi^2
aphi = {blah} / [AE/(B*cos(phi)) - B*cos(phi)]
{blah} = [-1/r - A/Bcos(phi)] * T
+ [-A/(B*cos(phi))] * [F*sin(phi)]
+ [-B*sin(phi)]*vphi^2
or aphi = {blah} / (B*cos(phi) - AE/(B*cos(phi)))
{blah} = (1/r + A/Bcos(phi)) * T
+ (AF/B)*tan(phi)
+ B*sin(phi)*vphi^2
btw, as an extra check we will simplify this aphi eqn now..
aphi = [(1/r + A/B) * T + (AF/B)*phi] / (B-AE/B)
cf aphi = [-(1/r+A/B)*T - (AF/B)*phi)] / (AE/B-B) //HOORAY, the same!
ok, lets plug in some numbers for the NON-linearised aphi..
{A = 81.607 B = 65.45 E = 74.332 F = (-641.41) r=0.37}
aphi = {blah} / (B*cos(phi) - AE/(B*cos(phi)))
aphi = {blah} / (65.45*cos(phi) - 92.68/cos(phi))
{blah} = (1/0.37 + (81.607/(65.45)/cos(phi))) * T
+ (81.607*(-641.41)/65.45)*tan(phi)
+ 65.45*sin(phi)*vphi^2
{blah} = (2.703 + 1.247/cos(phi)) * T
+ (-799.749)*tan(phi)
+ 65.45*sin(phi)*vphi^2
aphi = [(2.703+1.247/cos(phi))*T + (-799.749)*tan(phi) + 65.45*sin(phi)*vphi^2]
/ [65.45*cos(phi) - 92.68/cos(phi)] <=====*
now as an extra extra extra check, simplify the aphi numerics..
aphi = [(2.703+1.247)*T + (-799.749)*phi / [65.45*cos(phi) - 92.68]
aphi = [(3.95)*T + (-799.749)*phi / [-27.23]
aphi = [(-0.145)*T + (29.37)*phi //correct
Step 2:
T/r = A*ax + [B*cos(phi)]*aphi + [-B*sin(phi)]*vphi^2
AND aphi = [-1/E] * [T + [B*cos(phi)]*ax + [F*sin(phi)]]
SO T/r = A*ax + [B*cos(phi)] * [-1/E] * [T + [B*cos(phi)]*ax + [F*sin(phi)]]
+ [-B*sin(phi)]*vphi^2
T/r = A*ax
+ [B*cos(phi)]*[-1/E] * T
+ [B*cos(phi)]*[-1/E] * [B*cos(phi)]*ax
+ [B*cos(phi)]*[-1/E] * [F*sin(phi)]
+ [-B*sin(phi)]*vphi^2
A*ax + [B*cos(phi)]*[-1/E] * [B*cos(phi)]*ax =
T/r + [B*cos(phi)]*[1/E] * T
+ [B*cos(phi)]*[1/E] * [F*sin(phi)]
+ [B*sin(phi)]*vphi^2
ax = {stuff} / [A + [B*cos(phi)]*[-1/E] * [B*cos(phi)]]
{stuff} = [1/r + [(B/E)*cos(phi)]] * T
+ [(B/E)*cos(phi)] * [F*sin(phi)]
+ [B*sin(phi)]*vphi^2
ax = {stuff} / [A - [(B/E)*cos(phi)] * [B*cos(phi)]]
{stuff} = [1/r + (B/E)*cos(phi)] * T
+ (BF/E)*cos(phi)*sin(phi)
+ B*sin(phi)*vphi^2
ax = {stuff} / (A - (BB/E)*cos2(phi))
{stuff} = (1/r + (B/E)*cos(phi)) * T
+ (BF/E)*cos(phi)*sin(phi)
+ B*sin(phi)*vphi^2
btw, as an extra check we will simplify this ax eqn now..
ax = {(1/r + B/E) * T + (BF/E)*phi} / (A - (BB/E)
cf ax = [-(1/r+B/E)*T - (BF/E)*phi] / (BB/E-A) //HOORAY, the same!
ok, lets plug in some numbers for the NON-linearised ax..
{A = 81.607 B = 65.45 E = 74.332 F = (-641.41) r=0.37}
ax = {stuff} / (A - (BB/E)*cos2(phi))
ax = {stuff} / (81.607 - 57.629*cos2(phi))
{stuff} = (1/0.37 + (65.45/74.332)*cos(phi)) * T
+ (65.45*(-641.41)/74.332)*cos(phi)*sin(phi)
+ 65.45*sin(phi)*vphi^2
ax = [(2.703 + 0.881*cos(phi)) * T
+ (-564.767)*cos(phi)*sin(phi)
+ 65.45*sin(phi)*vphi^2] / (81.607 - 57.629*cos2(phi)) <=====*
now as an extra extra extra check, simplify the ax numerics..
ax = [(2.703 + 0.881) * T + (-564.767)*cos(phi)*sin(phi)]
/ (81.607 - 57.629)
ax = [(3.584) * T + (-564.767)phi]
/ (23.978)
ax = (0.149) * T + (-23.55)phi //correct
7. STANDARD SIMPLIFIED EQUATIONS
linearised/simplified..
T/r = m*ax + (I/r^2)*ax + M*ax + MR*aphi
-T = J*aphi + M*R^2*aphi + MR*ax - MR*g*phi
compare with fuzzy logic paper (gamma=0, after correcting for g):
identical - before and after linearising. HOORAY!
so lets make a big assumption that the gamma part of that paper is also ok.
write our equations (simplifying sin,cos for small angles):
T/r = [m+M+I/r^2]*ax + MR*(1-phi*gamma)*aphi
+ [-MR*(phi+gamma)]*vphi^2
+ [(m+M)*gamma*g]
-T = [MR*(1-phi*gamma)]*ax + [J+M*R^2]*aphi
+ [-MR*g]*phi
and take it one stage more simplified, dropping the vphi^2 for linearisation..
T/r = [m+M+I/r^2]*ax + MR*(1-phi*gamma)*aphi
+ [(m+M)*gamma*g]
-T = [MR*(1-phi*gamma)]*ax + [J+M*R^2]*aphi
+ [-MR*g]*phi
we prefer something like the following, with simple coefficients
for ax, aphi, vx, phi (which may be negative in some cases)..
T/r = A*ax + B*aphi + [C*vx + D] //dim=MLT-2
-T = B*ax + E*aphi + F*phi //dim=ML2T-2
rewrite as with constants..
A = [m+M+I/r^2] //coefficient of ax (eqn 1) dim=M
B = [MR*(1-phi*gamma)] //coefficient of aphi (eqn 1) dim=ML
// and coefficient of ax (eqn 2)
C = [0] //vx coefficient (hub friction?) dim=MT-1
D = [gamma*(m+M)*g] //stand alone dim=MLT-2
E = [J+MR^2] //coefficient of aphi (eqn 2) dim=ML2
F = [-MR*g] //coefficient of phi dim=ML2T-2
simplify further to avoid phi*gamma in B and E
we had cos(phi+gamma) ~ 1-phi*gamma
replace it by cos(gamma)
we could also replace by 1 but lets treat gamma as "fixed" parameter
rewritten..
A = [m+M+I/r^2] //coefficient of ax (eqn 1) dim=M
B = [MR*cos(gamma)] //coefficient of aphi (eqn 1) dim=ML
// and coefficient of ax (eqn 2)
C = [0] //vx coefficient (hub friction?) dim=MT-1
D = [gamma*(m+M)*g] //stand alone dim=MLT-2
E = [J+MR^2] //coefficient of aphi (eqn 2) dim=ML2
F = [-MR*g] //coefficient of phi dim=ML2T-2
or rewritten with zero slope..
A = [m+M+I/r^2] //coefficient of ax (eqn 1) dim=M
B = [MR] //coefficient of aphi (eqn 1) dim=ML
// and coefficient of ax (eqn 2)
C = [0] //vx coefficient (hub friction?) dim=MT-1
D = [0] //stand alone (zero slope) dim=MLT-2
E = [J+MR^2] //coefficient of aphi (eqn 2) dim=ML2
F = [-MR*g] //coefficient of phi dim=ML2T-2
anyway, our equations with simpler coefficients..
T/r = A*ax + B*aphi + [C*vx + D] //dim=MLT-2
-T = B*ax + E*aphi + F*phi //dim=ML2T-2
re-arrange 2nd eqn to get expressions for ax and for aphi..
ax = (-1/B)*(T+E*aphi+F*phi)
aphi = (-1/E)*(T+B*ax+F*phi)
so substitute ax in 1st equation (to get aphi)..
ax = [(-1/B)*T + (-E/B)*aphi + (-F/B)*phi]
T/r = A*[(-1/B)*T + (-E/B)*aphi + (-F/B)*phi] + B*aphi + [C*vx + D]
T/r = (-A/B)*T + (-EA/B)*aphi + (-FA/B)*phi + B*aphi + [C*vx + D]
(EA/B)*aphi+(B)*aphi = (-1/r-A/B)*T + (-FA/B)*phi + [C*vx + D]
aphi = [(-1/r-A/B)*T + (-FA/B)*phi + [C*vx + D]] / (EA/B-B)
or aphi = [-(1/r+A/B)*T - (AF/B)*phi + [C*vx + D]] / (AE/B-B)
and do the same ax substitution slightly differently as a check..
T/r = A*(-1/B)*(T+E*aphi+F*phi) + B*aphi + [C*vx + D]
A*(1/B)*(E*aphi) = -T/r + B*aphi + A*(-1/B)*(T+F*phi) + [C*vx + D]
A*(1/B)*(E*aphi) - B*aphi = -T/r + A*(-1/B)*(T+F*phi) + [C*vx + D]
(AE/B-B)*aphi = -T/r + A*(-1/B)*(T+F*phi) + [C*vx + D]
(AE/B-B)*aphi = -T/r - (A/B)*(T+F*phi) + [C*vx + D]
(AE/B-B)*aphi = -T/r - (A/B)*(T) - (A/B)*(F*phi) + [C*vx + D]
(AE/B-B)*aphi = -(1/r+A/B)*T - (F*A/B)*phi) + [C*vx + D]
aphi = [-(1/r+A/B)*T - (AF/B)*phi) + [C*vx + D]] / (AE/B-B) <<<-----$$
and also substitute aphi in 1st equation (to get ax)..
aphi = [(-1/E)*T + (-B/E)*ax + (-F/E)*phi)] from 2nd eqn
T/r = A*ax + B*[(-1/E)*T + (-B/E)*ax + (-F/E)*phi)] + [C*vx + D]
T/r = A*ax + (-B/E)*T + (-BB/E)*ax + (-BF/E)*phi) + [C*vx + D]
(BB/E-A)*ax = (-1/r)*T + (-B/E)*T + (-BF/E)*phi) + [C*vx + D]
(BB/E-A)*ax = -(B/E+1/r)*T - (BF/E)*phi) + [C*vx + D]
so ax = [-(1/r+B/E)*T - (BF/E)*phi) + [C*vx + D]] / (BB/E-A)
and do the same aphi substitution slightly differently as a check..
T/r = A*ax + B*(-1/E)*(T+B*ax+F*phi) + [C*vx + D]
-A*ax = -T/r + B*(-1/E)*(T+B*ax+F*phi) + [C*vx + D]
-A*ax = -T/r + B*(-1/E)*T + B*(-1/E)*B*ax + B*(-1/E)*F*phi + [C*vx + D]
-A*ax - B*(-1/E)*B*ax = -T/r + B*(-1/E)*T + B*(-1/E)*F*phi + [C*vx + D]
-A*ax + (BB/E)*ax= -T/r + B*(-1/E)*T + B*(-1/E)*F*phi + [C*vx + D]
(BB/E-A)*ax = -T/r - (B/E)*T - (BF/E)*phi + [C*vx + D]
(BB/E-A)*ax = -(B/E+1/r)*T - (BF/E)*phi + [C*vx + D]
ax = [-(1/r+B/E)*T - (BF/E)*phi + [C*vx + D]] / (BB/E-A) <<<------$$
AND we check the dimensions are correct..
dim A = M, dim B = ML
dim C = MT-1, dim D = MLT-2
dim E = ML2, dim F = ML2T-2
dim ax = LT-2
dim vx = LT-1
dim aphi = T-2
dim phi = _
dim T = ML2T-2,
dim rewrite aphi = [-(1/r+A/B)*T - (AF/B)*phi) + [C*vx + D]] / (AE/B-B)
(AE/B-B) = M.ML2/ML-ML = ML
so aphi*(AE/B-N) is MLT-2
T/r is MLT-2, T*A/N is ML2T-2.M/ML = MLT-2
phi*AF/B is M.ML2T-2/ML = MLT-2
C*vx is MT-1.LT-1 = MLT-2 and D is MLT-2
dim rewrite ax = [-(1/r+B/E)*T - (BF/E)*phi + [C*vx + D]] / (BB/E-A)
(BB/E-A) = ML.ML/ML2 - M = M
so ax*(BB/E-A) is LT-2.M = MLT-2
T/r is MLT-2, T*B/E is ML2T-2.ML/ML2 = MLT-2
phi*(BF/E) is ML.ML2T-2/ML2 = MLT-2
C*vx is MT-1.LT-1 = MLT-2 and D is MLT-2
ok, so that all looks good dimension wise!
now we are not usually using C and probably D is 0
now plug in some real world values to get A,B,,,E,F...
aphi = [-(1/r+A/B)*T - (AF/B)*phi) + [C*vx + D]] / (AE/B-B) <<<-----$$
ax = [-(1/r+B/E)*T - (BF/E)*phi + [C*vx + D]] / (BB/E-A) <<<------$$
repeating the numeric values from above, for convenience..
Tmax = 100Nm (rough rounded guess) eg 80kg * g on 125mm horiz crank
Tmin = -50Nm (it is harder to pedal backwards)
(and Tinmax = Tmax/2, Tinmin=Tmin/2)
g=9.8m/s^2
gamma = 0, so C = 0
friction zero (but see T), so D = 0
m = 3kg //mass of wheel
r = 0.37m //rolling radius of 29er
I = (2/3)*3*0.33^2 = 0.22kg_m^2 //approx
M = 77kg //mass of rider+frame
R = 0.85m //axle to just below belly button
J = (1/12)*77*((2*0.85)^2+3*0.10^2) = 18.7kg_m^2 //approx(bar radius 10cm)
now we need to compute the following..
A = 81.607 = [m+M+I/r^2] = 3+77+0.22/0.37^2
B = 65.45 = [MR] = 77*0.85
C = 0
D = 0
E = 74.332 = [J+MR^2] = 18.7+77*0.85^2
F = (-641.41) = [-MR*g] = -77*0.85*9.8
now rewrite the derived pair of acceleration equations..
aphi = j1*T + j2*phi + [j3*vx + j4]
ax = k1*T + k2*phi + [k3*vx + k4]
for convenience in subsequent calculations
(j1,j2,j3,j4,k1,k2,k3,k4) defined in terms of (A,B,C,D,E,F)..
j1 = -(1/r+A/B)/(AE/B-B)
j2 = -(AF/B)/(AE/B-B)
j3 = C/(AE/B-B)
j4 = D/(AE/B-B)
k1 = -(1/r+B/E)/(BB/E-A)
k2 = -(BF/E)/(BB/E-A)
k3 = C/(BB/E-A)
k4 = D/(BB/E-A)
now get the numeric values of these..
(AE/B-B) = (81.607*74.332/65.45 - 65.45) = 27.23
(BB/E-A) = (65.45*65.45/74.332 - 81.607) = -23.978
j1 = -(1/0.37+81.607/65.45)/27.23 = -0.145
j2 = -(81.607*(-641.41)/65.45)/27.23 = 29.37
k1 = -(1/0.37+65.45/74.332)/(-23.978) = 0.149
k2 = -(65.45*(-641.41)/74.332)/(-23.978) = -23.554
k3 = 0/(-23.978) = 0
k4 = 0/(-23.978) = 0
so here are our "final" equations (no slope, no hub friction):
ax = k1*T + k2*phi
aphi = j1*T + j2*phi
ax = 0.149*T + (-23.554)*phi
aphi = (-0.145)*T + 29.37*phi
8. A PRACTICAL NOTE
Consider the pedalling torque needed to
maintain a steady pitch angle of 3 degrees (0.052 radians)..
T = (0.052*29.37)/0.145 = 10.5Nm
giving ax = 0.149*10.533-23.554*0.052 = 0.35m/s^2
or for 1 degree (0.0175 radians)..
T = (0.0175*29.37)/0.145 = 3.5Nm
giving ax = 0.149*3.545-23.554*0.0175 = 0.12m/s^2
(which would achieve target 1m/s from rest in 8.3s)
and for about 12 degrees (approx 0.21 radians) ,
we need the average torque equivalent to max torque standing on pedal..
T = (0.21*29.37)/0.145 = 42.5Nm
giving ax = 0.149*42.53-23.554*0.21 = 1.39m/s^2
9. ADDING FRICTIONAL EFFECTS
neglect hub bearing friction.
neglect wind resistance as we are probably going slowly.
We MAY use a nominal road/tire rolling resistance force,
Tdrag, which is applied to T.
Near the balance point, Tdrag can be considered constant
(as normal force on wheel is approx constant and speed is slow)
eg Tdrag = (m+M)g*r*0.005 = (3+77)*9.8*0.37*0.005 = 1.45Nm cf Tmax = 100Nm?
Tdrag acts like a "hurdle" (non linear).
Tdrag is added or subtracted depending on the sign of Tpedal.
here Tpedal is the torque applied by the rider.
If |Tpedal| < Tdrag there can be no effective torque T for movement.
If using Tdrag, we will assume that the rider calculates the notional
effective torque T, then adds or subtracts Tdrag to get Tpedal at each
time k*dt where k=0,1,2,3,... and dt is perhaps 0.05s.
However, T will be remembered by the system, not Tpedal.
(also see Rayleigh's dissipation equation).
10. GENERAL SYSTEM CONTROL AND FEEDBACK
Incomplete notes in this section are applicable for a controller of a real system,
rather than a simulated human controller for a simulated physical system.
Please see section 13, "THE HUMAN RIDER MOTION CONTROL MODEL" below instead!
a standard system starts at t=0 and evolves as follows:
[dz/dt] = A.z + B.u
where z is a state vector and is [x,vx,phi,vphi] for us
and u is a control input vector and is [T] for us
state equation (expressed with our own variables) is:
ax 0 1 0 0 x 0
vx = 0 0 k2 0 * vx + T * k1
aphi 0 0 0 1 phi 0
vphi 0 0 j2 0 vphi j1
aside: the (unused by us) eigenvalues and eigenvectors of A are
(assuming k2=-23.554, j2=29.37):
d1 =-5.4194, d2=5.4194, d3=0
v1 = [0.148, -0.802, -0.1845, 1]
v2 = [-0.148, -0.802, 0.1845, 1)
If we were to go traditional matrix based control, we would have a matrix C
which generates our observed variables from full state,
and some noise vectors, and prove that the system is controllable,
and then introduce a feedback of both u, and and an estimated full state,
and push new complex eigenvalue into the stable area,
and compute a Kalman filter and an LQR controller.
[but we are not doing that].
our target state is [0, 1, 0, 0], ie steady vertical balanced at 1m/s forward.
our initial state may be (say) [0, 0, 0.01, 0.02].
This is a (SIMO) single input (T) multiple output (x,vx,phi,vphi)
We mainly care about phi (first priority is stay upright!).
The errors in phi, vphi and vx are available for feedback control.
The feedback is based on fixed parameters, but with the full knowledge of the
A and B matrices (and using current T value as well as observed state vector).
The actual driving torque might be further modified by applying a maximum limit,
and allowing for friction.
11. EVOLUTION OF STATE
Here we specifically use our equations for ZERO road slope,
and we have no vx dependent friction.
In particular we use the Euler-Lagrange derived motion equations for aphi and ax.
we use j prefix for the important phi dimension coefficients, and k for x dimension.
j1 and k1 are determined by mass or inertia opposing the acting torque.
j2 and k2 are determined primarily by the overturning gravity (~MRg).
aphi = j1*T + j2*phi [and numeric aphi = (-0.145)*T + 29.37*phi]
ax = k1*T + k2*phi [and numeric ax = 0.149*T + (-23.554)*phi]
some other constants..
xfuzz=0.05, vxfuzz=0.1, phifuzz=0.005, vphifuzz=0.01
The initial state values (at start of simulation, t=0) are:
x = 0
vx = 0
phi = 0.01 //launching with a slight forward lean
vphi = 0.02
T = 0 //the rider has not yet applied any driving torque
theta = pi/2 //pedals at 3 and 6 o'clock
imagine we are at time t, about to recompute [x,vx,phi,vphi]
and the last update was at t-dt..
first we save the existing (previous) state temporarily:
x_ = x
vx_ = vx
phi_ = phi
vphi_ = vphi
theta_ = theta
ax_ = ax
aphi_ = aphi
and recompute main agents of state change (accelerations)..
T = last rider control update of torque
aphi = j1*T + j2*phi_
ax = k1*T + k2*phi_
now update the state vector via these accelerations..
vx = vx_ + ax*dt
x = x_ + 0.5*(vx_+vx)*dt
vphi = vphi_ + aphi*dt
phi = phi_ + 0.5*(vphi_+vphi)*dt
and some secondary derived convenience output values..
theta = theta_ + (x - x_)/r
rpm = 9.549 * (theta - theta_)/dt //9.549=60/2pi
and then the observable state vector..
qx = x + random(0,xfuzz)
qvx = vx + random(0,vxfuzz)
qphi = phi + random(0,phifuzz)
qvphi = vphi + random(0,vphifuzz)
12. THE HUMAN SYSTEM CAPABILITIES
See link to paper (vestibular function, pitch..) giving the following info:
linear motion ("heave", unfortunately vertical - horiz is "sway" or "surge"!):
detection of small delta from 0m/s: 0.002m/s^2 -> 0.02m/s^2
detection of small delta from 1m/s: 0.21m/s^2 -> 0.24m/s^2
angular motion ("pitch") expressed in degrees:
detection of small delta from 0deg: 0.02deg/s^2 -> 0.30deg/s^2
detection of small delta from 20deg: 2.9deg/s^2 -> 3.4deg/s^2
angular motion ("pitch") expressed in radians:
detection of small delta from 0rad: 0.0003rad/s^2 -> 0.005rad/s^2
detection of small delta from 0.35rad: 0.05rad/s^2 -> 0.06rad/s^2
reaction time to out of balance signal: 0.1s -> 0.2s
13. THE HUMAN RIDER MOTION CONTROL MODEL
Input data for human controller comes from most recent state evolution:
t0 = time of last human control decision
t1 = time of last system state
t = current time
dt = t-t0
qx, qvx, qphi, qvphi = most recent updated observable state
qax and qaphi are unused
qT is unused
rpm = most recent derived rpm
theta = most recent pedal angle
The rider mainly senses/observes qphi, qvphi, qaphi and qrpm
(these are sensory interpretation of phi, vphi, aphi and rpm).
For practical purposes, on a smooth flat surface,
the axle torque needed is about 3.5Nm per degree of pitch,
to maintain balance at smallish pitch angles,
(and of course a bit more to avoid too much future forward movement
and to actually recover to 0 degrees).
So, in radians, 200Nm per radian to just achieve a balance (1rad ~ 57deg!).
So an observed error in phi (ie ephi = qphi-targetphi = qphi)
can be controlled by using Tin of about 200*ephi (extreme case).
This is without consideration (yet) of speed error,
erpm = qrpm-targetrpm.
Similarly, if we were ONLY concerned with achieving zero ax, ie maintaining
forward velocity (unlikely!), then a lesser torque would achieve this
(but at the expense of pitch increase!).
Control, if/when needed, is done by computing and applying a NEW value of Tin
depending mainly on ephi - bigger error means bigger Tin.
(and negative Tin if negative pitch error).
Human control is likely similar to a PID controller.
So there are 3 factors (but we use a 4th as well):
1. ephi(t) //directly sensed current phi = qphi (P)
2. hephi = weighted avg ephi(t-dt),ephi(t-2dt),ephi(t-3dt),.. //history/memory (I)
//using whephi as a weight, ie hephi = (ephi(k)+hephi*whephi)/(1+whephi)
3. dephi = change in ephi from last sample (D)
4. pephi = ephi+dt*qvphi (F) //predicted future ephi
These 4 values have associated multipliers kephi,khephi,kdephi,kpephi.
we normalise them so their sum of squares is 1.0.
and then we can calculate:
e1 = kephi*ephi + khephi*hephi + kdephi*dephi + kpephi*pephi.
(usually kephi will be largest weight).
so at last we can calculate Tin = ke1*e1 as balancing torque. (ke1 ~ 200)
but wait, there is more: (remember we have qrpm?)..
so we can calculate e2 = erpm = qrpm-targetrpm.
If e2 implies a significant rpm error,
but e1 is in the correct direction and not too much,
we have room to manouever - we "fake" a smaller e1,
so the correction torque will be initially less,
but later on will have to be more, thus changing rpm.
speed in the appropriate way.
ie from the rider's point of view:
to increase forward speed, first induce a forward pitch error,
then later need to pedal harder to increase rpm and recover balance.
so if conditions are right (some "if..then" logic on e1, e2),
we will add a positive or negative small e1delta to e1.
so then Tin = ke1 * (e1+e1delta)
Now apply an error to Tin before passing it to the main system..
Tin = Tin + random(Tfuzz)
And make sure the torque is within our (arbitrary) human limits..
calculate Tinmax and Tinmin based on crank angle theta.
(theta = pi/2 + x % (2*pi*r)) and has a minimum at the "dead point").
if (Tin>Tinmax) Tin = Tinmax
else if (Tin