diff --git a/lateral_track_analytic.py b/lateral_track_analytic.py index c0fedc5..f8e37dd 100644 --- a/lateral_track_analytic.py +++ b/lateral_track_analytic.py @@ -4,81 +4,118 @@ # build the bicycle state space b = {} -delta, deltaDot, phiDot, phi, psi, yP, yQ =\ - symbols('delta, deltaDot, phiDot, phi, psi, yP, yQ') -tPhi, tDelta, fB, tDeltaDot = symbols('tPhi, tDelta, fB, tDeltaDot') -b['x'] = Matrix([[yP], [psi], [phi], [delta], [phiDot], [deltaDot]]) -b['u'] = Matrix([[tPhi], [tDelta], [fB]]) -b['y'] = Matrix([[psi], [phi], [delta], [phiDot], [yQ]]) - -numStates = len(b['x']) -numInputs = len(b['u']) -numOutputs = len(b['y']) +delta, deltadot, phidot, phi, psi, yP, yq = symbols( + 'delta, deltadot, phidot, phi, psi, yP, yq') +tphi, tdelta, fB, tdeltadot = symbols('tphi, tdelta, fB, tdeltadot') +b['x'] = Matrix([ + [phi], + [delta], + [phidot], + [deltadot], + [psi], + [yP], +]) +b['u'] = Matrix([ + [tphi], + [tdelta], + [fB], +]) +b['y'] = Matrix([ + [phi], + [delta], + [phidot], + [psi], + [yq], +]) + +num_states = len(b['x']) +num_inputs = len(b['u']) +num_outputs = len(b['y']) # Make the A and B matrices generic as they are just essentially submatrices in # the closed loop system, but the C matrix plays a role in controller equations # because we are tracking the lateral deviation of the front wheel contact. In # this case, set the appropriate entries that are ones and zeros so that the # final equatios will be in a reasonably understandable form. -b['A'] = Matrix(numStates, numStates, - lambda i, j: Symbol('a' + str(i + 1) + str(j + 1))) - -b['B'] = Matrix(numStates, numInputs, - lambda i, j: Symbol('b' + str(i + 1) + str(j + 1))) - -#b['C'] = Matrix(numOutputs, numStates, - #lambda i, j: Symbol('c' + str(i + 1) + str(j + 1))) - -b['C'] = zeros((numOutputs, numStates)) -b['C'][0, 1] = 1 # psi -b['C'][1, 2] = 1 # phi -b['C'][2, 3] = 1 # delta -b['C'][3, 4] = 1 # phiDot -# yQ depends on the rear wheel location, the heading angle and the steer angle -b['C'][4, 0] = 1 -c52, c54 = symbols('c52 c54') -b['C'][4, 1] = c52 -b['C'][4, 3] = c54 - -b['D'] = Matrix(numOutputs, numInputs, lambda i, j: 0.) +b['A'] = zeros(num_states, num_states) +b['A'][0, 2] = 1 +b['A'][1, 3] = 1 +b['A'][2, 0] = Symbol('a_phidd_phi') +b['A'][2, 1] = Symbol('a_phidd_del') +b['A'][2, 2] = Symbol('a_phidd_phid') +b['A'][2, 3] = Symbol('a_phidd_deld') +b['A'][3, 0] = Symbol('a_deldd_phi') +b['A'][3, 1] = Symbol('a_deldd_del') +b['A'][3, 2] = Symbol('a_deldd_phid') +b['A'][3, 3] = Symbol('a_deldd_deld') +b['A'][4, 1] = Symbol('a_psid_del') +b['A'][4, 3] = Symbol('a_psid_deld') +b['A'][5, 4] = Symbol('a_ypd_psi') + +b['B'] = zeros(num_states, num_inputs) +b['B'][2, 1] = Symbol('b_phidd_Tdel') +b['B'][2, 2] = Symbol('b_phidd_F') +b['B'][3, 1] = Symbol('b_deldd_Tdel') +b['B'][3, 2] = Symbol('b_deldd_F') + +b['C'] = zeros(num_outputs, num_states) +b['C'][0, 0] = 1 # phi +b['C'][1, 1] = 1 # delta +b['C'][2, 2] = 1 # phidot +b['C'][3, 4] = 1 # psi +# yq depends on the rear wheel location, the heading angle and the steer angle +b['C'][4, 1] = Symbol('c_yq_del') +b['C'][4, 4] = Symbol('c_yq_psi') +b['C'][4, 5] = 1 + +b['D'] = Matrix(num_outputs, num_inputs, lambda i, j: 0.) # Write the controller output as a function of the gains and the commanded # lateral deviation. -kDelta, kPhiDot, kPhi, kPsi, kYQ = symbols('kDelta, kPhiDot, kPhi, kPsi, kYQ') -deltac, phiDotc, phic, psic, yc = symbols('deltac, phiDotc, phic, psic, yc') -psic = kYQ * (yc - yQ) -phic = kPsi * (psic - psi) -phiDotc = kPhi * (phic - phi) -deltac = kPhiDot * (phiDotc - phiDot) -Unm = kDelta * (deltac - delta) +kdelta, kphidot, kphi, kpsi, kyq = symbols('kdelta, kphidot, kphi, kpsi, kyq') +deltac, phidotc, phic, psic, yc = symbols('deltac, phidotc, phic, psic, yc') +psic = kyq*(yc - yq) +phic = kpsi*(psic - psi) +phidotc = kphi*(phic - phi) +deltac = kphidot*(phidotc - phidot) +Unm = kdelta*(deltac - delta) Unm = Unm.expand() -controller = Matrix([Unm.coeff(psi), Unm.coeff(phi), Unm.coeff(delta), - Unm.coeff(phiDot), Unm.coeff(yQ), Unm.coeff(yc)]).T +controller = Matrix([ + Unm.coeff(phi), + Unm.coeff(delta), + Unm.coeff(phidot), + Unm.coeff(psi), + Unm.coeff(yq), + Unm.coeff(yc), +]).T # neuromuscular state space -omega = Symbol('w') +omega = Symbol('omega') zeta = Symbol('zeta') n = {} n['A'] = Matrix([[0, 1], - [-omega**2, -2 * zeta * omega]]) + [-omega**2, -2*zeta*omega]]) n['B'] = Matrix([[0], [omega**2]]) n['C'] = Matrix([1, 0]).T n['D'] = Matrix([0]) -n['x'] = Matrix([[tDelta], - [tDeltaDot]]) +n['x'] = Matrix([[tdelta], + [tdeltadot]]) n['u'] = Matrix([Unm]) -n['y'] = Matrix([tDelta]) +n['y'] = Matrix([tdelta]) # Compute the differential equations for the closed loop system. -xDot = b['A'] * b['x'] + b['B'] * b['u'] -xDot = xDot.col_join(n['A'] * n['x'] + n['B'] * controller * (b['C'] * - b['x']).col_join(Matrix([yc]))) +xdot = b['A']*b['x'] + b['B']*b['u'] +xdot = xdot.col_join( + n['A']*n['x'] + n['B']*controller*(b['C']*b['x']).col_join( + Matrix([yc]))) +y = (b['C']*b['x']).col_join(n['C']*n['x']) # Build the closed loop state space matrices. -systemStates = [yP, psi, phi, delta, phiDot, deltaDot, tDelta, tDeltaDot] -systemInputs = [tPhi, fB, yc] -systemOutputs = [psi, phi, delta, phiDot, yQ, tDelta] +system_states = [phi, delta, phidot, deltadot, psi, yP, tdelta, tdeltadot] +system_inputs = [fB, yc] +system_outputs = [phi, delta, phidot, psi, yq, tdelta] + def mat_coeff(equations, variables, i, j): c = equations[i].expand().coeff(variables[j]) @@ -86,13 +123,12 @@ def mat_coeff(equations, variables, i, j): c = 0 return c -A = Matrix(len(systemStates), len(systemStates), lambda i, j: mat_coeff(xDot, - systemStates, i, j)) -B = Matrix(len(systemStates), len(systemInputs), lambda i, j: mat_coeff(xDot, - systemInputs, i, j)) +A = Matrix(len(system_states), len(system_states), + lambda i, j: mat_coeff(xdot, system_states, i, j)) -y = (b['C'] * b['x']).col_join(n['C'] * n['x']) +B = Matrix(len(system_states), len(system_inputs), + lambda i, j: mat_coeff(xdot, system_inputs, i, j)) -C = Matrix(len(systemOutputs), len(systemStates), lambda i, j: mat_coeff(y, - systemStates, i, j)) +C = Matrix(len(system_outputs), len(system_states), + lambda i, j: mat_coeff(y, system_states, i, j)) diff --git a/simulate_controlled_system.py b/simulate_controlled_system.py new file mode 100644 index 0000000..c5ca269 --- /dev/null +++ b/simulate_controlled_system.py @@ -0,0 +1,77 @@ +import sympy as sm +import numpy as np +from scipy.signal import lsim, lti +import matplotlib.pyplot as plt + +from lateral_track_analytic import (A, B, C, system_outputs) + +args = list(sm.ordered(list(A.free_symbols | B.free_symbols | C.free_symbols))) + +eval_sys = sm.lambdify(args, (A, B, C)) + +# q'' = Minv(-(g*K0 + v**2*K2)*q - v*C1*q' + T + H*F)) +# Bicycle 1 from Hess, Moore, Hubbard 2012 +M = np.array([[106.0, 1.55], [1.55, 0.25]]) +K0 = np.array([[-93.2, -1.76], [-1.76, -0.68]]) +K2 = np.array([[0.0, 77.3], [0.0, 1.58]]) +C1 = np.array([[0.0, 29.9], [-0.45, 1.08]]) +v, g = 5.0, 9.81 +c, w, lam = 0.068, 1.12, 0.4 +# H from Jason in Moore 2012 (should be Bicycle 1 values to be perfectly +# correct) +H = np.array([[0.943], [0.011]]) +# Bicycle plant state space +Minv = np.linalg.inv(M) +# x = phi, del, phid, deld +Ab = np.block([ + [np.zeros_like(M), np.eye(2)], + [-Minv@(g*K0 + v**2*K2), -Minv*v@C1] +]) +# u = Tphi, Tdel, F +Bb = np.block([ + [np.zeros_like(M), np.zeros((2, 1))], + [Minv, Minv@H] +]) + +vals = np.array([ + Ab[3, 1], # a_deldd_del, + Ab[3, 3], # a_deldd_deld, + Ab[3, 0], # a_deldd_phi, + Ab[3, 2], # a_deldd_phid, + Ab[2, 1], # a_phidd_del, + Ab[2, 3], # a_phidd_deld, + Ab[2, 0], # a_phidd_phi, + Ab[2, 2], # a_phidd_phid, + v/w*np.cos(lam), # a_psid_del, + c/w*np.cos(lam), # a_psid_deld, + v, # a_ypd_psi, + Bb[3, 2], # b_deldd_F, + Bb[3, 1], # b_deldd_Tdel, + Bb[2, 2], # b_phidd_F, + Bb[2, 1], # b_phidd_Tdel, + -c*np.cos(lam), # c_yq_del, + w, # c_yq_psi, + 48.0, # kDelta, + 9.03, # kPhi, + -0.08, # kPhiDot, + 0.161, # kPsi, + 0.097, # kYQ, + 30.0, # omega + 0.707, # zeta +]) + +A_cl, B_cl, C_cl = eval_sys(*vals) + +sys = lti(A_cl, B_cl, C_cl, np.zeros((C_cl.shape[0], B_cl.shape[1]))) +t = np.linspace(0.0, 5.0, num=100) +u = np.zeros((len(t), B_cl.shape[1])) +u[len(t)//4:len(t)//3, 0] = 20.0 # F +u[:, 1] = 0.2 # yc + +t, y, x = lsim(sys, u, t) + +fig, axes = plt.subplots(nrows=y.shape[1]) +for yi, ax, lab in zip(y.T, axes, system_outputs): + ax.plot(t, yi, label=lab) + ax.legend() +plt.show()