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