spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

lat_control_steer_model: penalize delta_torque: 

optain torque as the solution of:

      min_(\delta_torque) norm(angle - desired_angle, dangle - desired_angle_rate, torque_current+\delta_torque, delta_torque)_W

with torque_next = torque_current + delta_torque
angle, dangle = euler forward simulation with model and (torque_next)
steering_model
Jonathan Frey 2022-02-24 16:54:36 +01:00
parent 749078d4d9
commit 319ffdcedc
1 changed files with 22 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
selfdrive/controls/lib/latcontrol_steer_model.py
View File

@ -27,11 +27,8 @@ class LatControlSteerModel(LatControl):
self.B = model_param[NX*NX:NX*(NX+NU)].reshape((NX, NU), order='F')
self.R = model_param[NX*(NX+NU):].reshape((NX, N_live_param), order='F')
# print(f"LatControlSteerModel: \nA {self.A}\nB {self.B}\nR {self.R}\n")
self.W = np.diag([1e0, 0.05, 0.2])
# self.PHI = (DT_CTRL * self.B).reshape((2,1))
# self.M_tilde = - 1/(self.PHI.T @ self.W @ self.PHI) * (self.PHI.T@self.W)
# weights of (angle, dangle, torque, delta_torque)
self.W = np.diag([1., 0.2, .02, 10.])
self.torque = 0.0
def reset(self):
@ -64,44 +61,32 @@ class LatControlSteerModel(LatControl):
# NOTE: controls_list dependent.
u = np.array([last_actuators.steer, last_actuators.steer*abs(last_actuators.steer), last_actuators.steer/(CS.vEgo**2)])
# update state estimate with forward simulation
self.xcurrent = self.xcurrent + DT_CTRL * (self.A @ self.xcurrent + Rp + (self.B @ u))
dt_AxpRppBu = DT_CTRL * (self.A @ self.xcurrent + Rp + (self.B @ u))
self.xcurrent = self.xcurrent + dt_AxpRppBu
# analytical solution similar to steady state.
# steady_state_torque = self.M * (self.A @ self.xcurrent + self.R @ live_param) # solve for xdot = 0
# torque = argmin norm([desired_angle, xcurrent_1] + DT_CTRL * (A*xcurrent + R*live_param + B*u))
# torque_np = self.M @ (self.A @ self.xcurrent + self.R @ live_param +
# (self.xcurrent - np.array([angle_steers_des_no_offset, self.xcurrent[1,]]))/DT_CTRL )
#
# torque = argmin norm(xcurrent + DT_CTRL * (A*xcurrent + R*live_param + B*u) - [desired_angle, desired_angle_rate])_W
# NOTE: controls_list dependent.
du_dtorque = np.array([1, 2*abs(self.torque), 1/(CS.vEgo**2)])
B_tilde = self.B @ du_dtorque
desired_angle_rate = math.degrees(VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0.0))
Phi = (DT_CTRL * B_tilde).reshape((2,1))
# M_tilde = - 1/(Phi.T @ self.W @ Phi) * (Phi.T@self.W)
Phi_tilde = np.zeros((3,1))
Phi_tilde[:2] = Phi
Phi_tilde[-1] = 1.0
# SOLVE optimization problem:
# min_(\delta_torque) norm(angle - desired_angle, dangle - desired_angle_rate, torque_current+\delta_torque, delta_torque)_W
# with torque_next = torque_current + delta_torque
# angle, dangle = euler forward simulation with model and (torque_next)
y = np.zeros((4,))
# actual values
y[:2] = self.xcurrent
y[2] = last_actuators.steer
# subtract desired values
y[0] -= angle_steers_des_no_offset
y[1] -= desired_angle_rate
# fix contribution dynamic system
y[:2] += dt_AxpRppBu
#
Phi_tilde = np.append(DT_CTRL * B_tilde, [1.0, 1.0] )
M_tilde = - 1/(Phi_tilde.T @ self.W @ Phi_tilde) * (Phi_tilde.T@self.W)
y = (self.xcurrent + DT_CTRL * (self.A @ self.xcurrent + Rp) -
np.array([angle_steers_des_no_offset, desired_angle_rate])).reshape((2,1))
y_tilde = np.zeros((3,1))
y_tilde[:2] = y
y_tilde[-1] = 0.0
torque_np = M_tilde @ y_tilde
# hacky but works well..
# B_tilde = B_tilde.reshape((2,1))
# M = - np.linalg.solve(B_tilde.T @ B_tilde, B_tilde.T)
# torque_np = M @ (self.A @ self.xcurrent + self.R @ live_param +
# (self.xcurrent - np.array([angle_steers_des_no_offset, .95*self.xcurrent[1,]]))/DT_CTRL )
# steady state torque: for x = [angle_steers_des_no_offset, desired_angle_rate]
# M = - np.linalg.solve(B_tilde.T @ B_tilde, B_tilde.T)
# torque_np = M @ (self.A @ np.array([angle_steers_des_no_offset, self.xcurrent[1]] ) + Rp)
delta_torque = M_tilde @ y
torque_np = last_actuators.steer + delta_torque
output_steer = float(torque_np)
self.torque = output_steer