control.py: PID accounts for timestep length, previously assumed =1 (fb0fef64) · Commits · Ibrahim Ahmed / multirotor

control.py

+42 −15

Original line number	Diff line number	Diff line
		from dataclasses import dataclass

		import numpy as np
		from scipy.integrate import trapezoid

		from .simulation import Multirotor
		from .coords import euler_to_angular_rate



		@dataclass
		class PIDController:
		"""
		Proportional Integral Derivative controller. Tracks a reference signal and outputs
		a control signal to minimize output. The equation is:

		err = reference - measurement

		u = k_p * err + k_d * d(err)/dt + k_i * int(err . dt)
		"""

		k_p: np.ndarray
		"Proportional constant"
		k_i: np.ndarray
		"Integral constant"
		k_d: np.ndarray
		"Derivative constant"
		max_err_i: np.ndarray
		"Maximum accumulated error"
		dt: float
		"Simulation parameters to set timestep"

		def __post_init__(self):
		self.err_p = np.zeros_like(self.k_p)
		self.err_i = np.zeros_like(self.k_i)
		self.err_d = np.zeros_like(self.k_d)
		self.err = 0.
		self.err = np.zeros_like(self.k_p)
		if self.max_err_i is None:
		self.max_err_i = np.inf


		def reset(self):
		self.err = 0.
		self.err *= 0
		self.err_p *= 0
		self.err_i *= 0
		self.err_d *= 0
		@@ -33,8 +49,11 @@ class PIDController:
		def step(self, reference: np.ndarray, measurement: np.ndarray) -> np.ndarray:
		err = reference - measurement
		self.err_p = err
		self.err_i = np.clip(self.err_i + err, a_min=-self.max_err_i, a_max=self.max_err_i)
		self.err_d = err - self.err
		self.err_i = np.clip(
		self.err_i + trapezoid((self.err, err), dx=self.dt, axis=0),
		a_min=-self.max_err_i, a_max=self.max_err_i
		)
		self.err_d = (err - self.err) / self.dt
		self.err = err
		return self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d

		@@ -52,13 +71,7 @@ class PosController(PIDController):
		self.k_p = np.ones(2) * np.asarray(self.k_p)
		self.k_i = np.ones(2) * np.asarray(self.k_i)
		self.k_d = np.ones(2) * np.asarray(self.k_d)

		self.err_p = np.zeros_like(self.k_p)
		self.err_i = np.zeros_like(self.k_i)
		self.err_d = np.zeros_like(self.k_d)
		self.err = 0.
		if self.max_err_i is None:
		self.max_err_i = np.inf
		super().__post_init__()


		def step(self, reference, measurement):
		@@ -73,9 +86,9 @@ class PosController(PIDController):
		# desired/reference change in x/y i.e. velocity
		ref_delta_xy = rot @ np.asarray([delta_x, delta_y])
		# TODO: Explicitly track velocity, instead of deltas
		# ref_vel_xy = ref_delta_xy / self.vehicle.simulation.dt
		# abs_max_vel = np.abs((ref_vel_xy / (np.linalg.norm(ref_vel_xy) + 1e-6)) * self.max_velocity)
		# ref_vel_xy = np.clip(ref_vel_xy, a_min=-abs_max_vel, a_max=abs_max_vel)
		ref_vel_xy = ref_delta_xy / self.vehicle.simulation.dt
		abs_max_vel = np.abs((ref_vel_xy / (np.linalg.norm(ref_vel_xy) + 1e-6)) * self.max_velocity)
		ref_vel_xy = np.clip(ref_vel_xy, a_min=-abs_max_vel, a_max=abs_max_vel)
		# actual/measured velocity
		mea_delta_xy = mea_vel_xy = self.vehicle.velocity[:2]
		# desired pitch, roll
		@@ -94,9 +107,16 @@ class AttController(PIDController):
		vehicle: Multirotor


		def __post_init__(self):
		self.k_p = np.ones(3) * np.asarray(self.k_p)
		self.k_i = np.ones(3) * np.asarray(self.k_i)
		self.k_d = np.ones(3) * np.asarray(self.k_d)
		super().__post_init__()


		def step(self, reference, measurement):
		# desired change in orientation i.e. angular velocity
		ref_delta = reference - measurement
		ref_delta = euler_to_angular_rate(reference - measurement, self.vehicle.orientation)
		# actual change in orientation
		mea_delta = self.vehicle.angular_rate
		# prescribed change in velocity i.e. angular acceleration
		@@ -136,6 +156,13 @@ class Controller:
		assert self.ctrl_a.vehicle is self.ctrl_p.vehicle, "Vehicle instances different."
		assert self.ctrl_a.vehicle is self.ctrl_z.vehicle, "Vehicle instances different."


		def reset(self):
		self.ctrl_a.reset()
		self.ctrl_p.reset()
		self.ctrl_z.reset()


		def step(self, reference, measurement=None):
		# x,y,z,yaw
		pitch_roll = self.ctrl_p.step(reference[:2], self.vehicle.position[:2])

Admin message