Source code for roboticstoolbox.blocks.uav

import numpy as np
from math import sin, cos, atan2, tan, sqrt, pi

import matplotlib.pyplot as plt
import time

from bdsim.components import TransferBlock, FunctionBlock
from bdsim.graphics import GraphicsBlock
# ------------------------------------------------------------------------ #

[docs]class MultiRotor(TransferBlock): """ :blockname:`MULTIROTOR` .. table:: :align: left +------------+---------+---------+ | inputs | outputs | states | +------------+---------+---------+ | 1 | 1 | 16 | +------------+---------+---------+ | A(4,) | dict | | +------------+---------+---------+ """ nin = 1 nout = 1 # Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04 # A simulation of idealised X-4 Flyer II flight dynamics. # version 2.0 2005 modified to be compatible with latest version of Matlab # version 3.0 2006 fixed rotation matrix problem # version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring # version 5.0 8/8/11, simplified and restructured # version 6.0 25/10/13, fixed rotation matrix/inverse wronskian definitions, flapping cross-product bug # # New in version 2: # - Generalised rotor thrust model # - Rotor flapping model # - Frame aerodynamic drag model # - Frame aerodynamic surfaces model # - Internal motor model # - Much coolage # # Version 1.3 # - Rigid body dynamic model # - Rotor gyroscopic model # - External motor model # # ARGUMENTS # u Reference inputs 1x4 # tele Enable telemetry (1 or 0) 1x1 # crash Enable crash detection (1 or 0) 1x1 # init Initial conditions 1x12 # # INPUTS # u = [N S E W] # NSEW motor commands 1x4 # # CONTINUOUS STATES # z Position 3x1 (x,y,z) # v Velocity 3x1 (xd,yd,zd) # n Attitude 3x1 (Y,P,R) # o Angular velocity 3x1 (wx,wy,wz) # w Rotor angular velocity 4x1 # # Notes: z-axis downward so altitude is -z(3) # # CONTINUOUS STATE MATRIX MAPPING # x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4] # # # CONTINUOUS STATE EQUATIONS # z` = v # v` = g*e3 - (1/m)*T*R*e3 # I*o` = -o X I*o + G + torq # R = f(n) # n` = inv(W)*o #
[docs] def __init__(self, model, groundcheck=True, speedcheck=True, x0=None, **blockargs): r""" Create a a multi-rotor dynamic model block. :param model: Vehicle geometric and inertial parameters :type model: dict :param groundcheck: Prevent vehicle moving below ground, defaults to True :type groundcheck: bool :param speedcheck: Check for zero rotor speed, defaults to True :type speedcheck: bool :param x0: Initial state, defaults to None :type x0: float, optional :param blockargs: |BlockOptions| :type blockargs: dict :return: a MULTIROTOR block :rtype: MultiRotor instance **Block ports** :input ω: a vector of input rotor speeds in (radians/sec). These are, looking down, clockwise from the front rotor which lies on the x-axis. :output x: a dictionary signal with the following items: - ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]` - ``vb`` translational velocity in the world frame (metres/sec) - ``w`` angular rates in the world frame as yaw-pitch-roll rates (radians/second) - ``a1s`` longitudinal flapping angles (radians) - ``b1s`` lateral flapping angles (radians) Based on MATLAB code developed by Pauline Pounds 2004. """ super().__init__(nin=1, nout=1, **blockargs) self.type = 'quadrotor' try: nrotors = model['nrotors'] except KeyError: raise RuntimeError('vehicle model does not contain nrotors') assert nrotors % 2 == 0, 'Must have an even number of rotors' self.nstates = 12 if x0 is not None: assert len(x0) == self.nstates, "x0 is the wrong length" else: x0 = np.zeros((self.nstates,)) self._x0 = x0 self.nrotors = nrotors self.model = model self.groundcheck = groundcheck self.speedcheck = speedcheck self.D = np.zeros((3,self.nrotors)) self.theta = np.zeros((self.nrotors,)) for i in range(0, self.nrotors): theta = i / self.nrotors * 2 * pi # Di Rotor hub displacements (1x3) # first rotor is on the x-axis, clockwise order looking down from above self.D[:,i] = np.r_[ model['d'] * cos(theta), model['d'] * sin(theta), model['h']] self.theta[i] = theta self.a1s = np.zeros((self.nrotors,)) self.b1s = np.zeros((self.nrotors,))
def output(self, t=None): model = self.model # compute output vector as a function of state vector # z Position 3x1 (x,y,z) # v Velocity 3x1 (xd,yd,zd) # n Attitude 3x1 (Y,P,R) # o Angular velocity 3x1 (Yd,Pd,Rd) n = self._x[3:6] # RPY angles phi = n[0] # yaw the = n[1] # pitch psi = n[2] # roll # rotz(phi)*roty(the)*rotx(psi) # BBF > Inertial rotation matrix R = np.array([ [cos(the) * cos(phi), sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)], [cos(the) * sin(phi), sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi), cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi)], [-sin(the), sin(psi) * cos(the), cos(psi) * cos(the)] ]) #inverted Wronskian iW = np.array([ [0, sin(psi), cos(psi)], [0, cos(psi) * cos(the), -sin(psi) * cos(the)], [cos(the), sin(psi) * sin(the), cos(psi) * sin(the)] ]) / cos(the) # return velocity in the body frame out = {} out['x'] = self._x[0:6] out['vb'] = np.linalg.inv(R) @ self._x[6:9] # translational velocity mapped to body frame out['w'] = iW @ self._x[9:12] # RPY rates mapped to body frame out['a1s'] = self.a1s out['b1s'] = self.b1s out['X'] = self._x return [out] def deriv(self): model = self.model # Body-fixed frame references # ei Body fixed frame references 3x1 e3 = np.r_[0, 0, 1] # process inputs w = self.inputs[0] if len(w) != self.nrotors: raise RuntimeError('input vector wrong size') if self.speedcheck and np.any(w == 0): # might need to fix this, preculudes aerobatics :( # mu becomes NaN due to 0/0 raise RuntimeError('quadrotor_dynamics: not defined for zero rotor speed'); # EXTRACT STATES FROM X z = self._x[0:3] # position in {W} n = self._x[3:6] # RPY angles {W} v = self._x[6:9] # velocity in {W} o = self._x[9:12] # angular velocity in {W} # PREPROCESS ROTATION AND WRONSKIAN MATRICIES phi = n[0] # yaw the = n[1] # pitch psi = n[2] # roll # rotz(phi)*roty(the)*rotx(psi) # BBF > Inertial rotation matrix R = np.array([ [cos(the)*cos(phi), sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi), cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi)], [cos(the)*sin(phi), sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi), cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi)], [-sin(the), sin(psi)*cos(the), cos(psi)*cos(the)] ]) # Manual Construction # Q3 = [cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1]; % RZ %Rotation mappings # Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; % RY # Q1 = [1 0 0;0 cos(psi) -sin(psi);0 sin(psi) cos(psi)]; % RX # R = Q3*Q2*Q1 %Rotation matrix # # RZ * RY * RX # inverted Wronskian iW = np.array([ [0, sin(psi), cos(psi)], [0, cos(psi)*cos(the), -sin(psi)*cos(the)], [cos(the), sin(psi)*sin(the), cos(psi)*sin(the)] ]) / cos(the) # ROTOR MODEL T = np.zeros((3,4)) Q = np.zeros((3,4)) tau = np.zeros((3,4)) a1s = self.a1s b1s = self.b1s for i in range(0, self.nrotors): # for each rotor # Relative motion Vr = np.cross(o, self.D[:,i]) + v mu = sqrt(np.sum(Vr[0:2]**2)) / (abs(w[i]) * model['r']) # Magnitude of mu, planar components lc = Vr[2] / (abs(w[i]) * model['r']) # Non-dimensionalised normal inflow li = mu # Non-dimensionalised induced velocity approximation alphas = atan2(lc, mu) j = atan2(Vr[1], Vr[0]) # Sideslip azimuth relative to e1 (zero over nose) J = np.array([ [cos(j), -sin(j)], [sin(j), cos(j)] ]) # BBF > mu sideslip rotation matrix # Flapping beta = np.array([ [((8/3*model['theta0'] + 2 * model['theta1']) * mu - 2 * lc * mu) / (1 - mu**2 / 2)], # Longitudinal flapping [0] # Lattitudinal flapping (note sign) ]) # sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)]; beta = J.T @ beta; # Rotate the beta flapping angles to longitudinal and lateral coordinates. a1s[i] = beta[0] - 16 / model['gamma'] / abs(w[i]) * o[1] b1s[i] = beta[1] - 16 / model['gamma'] / abs(w[i]) * o[0] # Forces and torques # Rotor thrust, linearised angle approximations T[:,i] = model['Ct'] * model['rho'] * model['A'] * model['r']**2 * w[i]**2 * \ np.r_[-cos(b1s[i]) * sin(a1s[i]), sin(b1s[i]), -cos(a1s[i])*cos(b1s[i])] # Rotor drag torque - note that this preserves w[i] direction sign Q[:,i] = -model['Cq'] * model['rho'] * model['A'] * model['r']**3 * w[i] * abs(w[i])* e3 tau[:,i] = np.cross(T[:,i], self.D[:,i]) # Torque due to rotor thrust # RIGID BODY DYNAMIC MODEL dz = v dn = iW @ o dv = model['g'] * e3 + R @ np.sum(T, axis=1) / model['M'] # vehicle can't fall below ground, remember z is down if self.groundcheck and z[2] > 0: z[0] = 0 dz[0] = 0 do = np.linalg.inv(model['J']) @ (np.cross(-o, model['J'] @ o) + np.sum(tau, axis=1) + np.sum(Q, axis=1)) # row sum of torques # # stash the flapping information for plotting # self.a1s = a1s # self.b1s = b1s return np.r_[dz, dn, dv, do] # This is the state derivative vector
# ------------------------------------------------------------------------ #
[docs]class MultiRotorMixer(FunctionBlock): """ :blockname:`MULTIROTORMIXER` .. table:: :align: left +--------+---------+---------+ | inputs | outputs | states | +--------+---------+---------+ | 4 | 1 | 0 | +--------+---------+---------+ | float | | | +--------+---------+---------+ """ nin = 4 nout = 1 inlabels = ('𝛕r', '𝛕p', '𝛕y', 'T') outlabels = ('ω',)
[docs] def __init__(self, maxw=1000, minw=5, **blockargs): """ Create a block that displays/animates a multi-rotor flying vehicle. :param maxw: maximum rotor speed in rad/s, defaults to 1000 :type maxw: float :param minw: minimum rotor speed in rad/s, defaults to 5 :type minw: float :param blockargs: |BlockOptions| :type blockargs: dict :return: a MULTIROTORMIXER block :rtype: MultiRotorMixer instance **Block ports** :input 𝛕r: roll torque :input 𝛕p: pitch torque :input 𝛕y: yaw torque :input T: total thrust :output ω: 1D array of rotor speeds Derived from Simulink model by Pauline Pounds 2004 """ super().__init__(inputs=inputs, **blockargs) self.type = 'multirotormixer' self.minw = minw self.maxw = maxw
def output(self, t): w = np.zeros((self.nrotors,)) tau = self.inputs for i in self.nrotors: # roll and pitch coupling w[i] += -tau[0] * sin(self.theta[i]) + tau[1] * cos(self.theta[i]) # yaw coupling sign = 1 if i % 1 == 0 else -1 w[i] += sign * tau[2] # overall thrust w[i] += tau[3] / self.nrotors # clip the rotor speeds to the range [minw, maxw] w = np.clip(w, self.minw, self.maxw) # convert to thrust w = np.sqrt(w) / self.model['b'] # negate alterate rotors to indicate counter-rotation for i in self.nrotors: if i % 1 == 0: w[i] = -w[i] return [w]
# ------------------------------------------------------------------------ #
[docs]class MultiRotorPlot(GraphicsBlock): """ :blockname:`MULTIROTORPLOT` .. table:: :align: left +--------+---------+---------+ | inputs | outputs | states | +--------+---------+---------+ | 1 | 0 | 0 | +--------+---------+---------+ | dict | | | +--------+---------+---------+ """ nin = 1 nout = 0 inlabels = ('x',) # Based on code lovingly coded by Paul Pounds, first coded 17/4/02 # version 2 2004 added scaling and ground display # version 3 2010 improved rotor rendering and fixed mirroring bug # Displays X-4 flyer position and attitude in a 3D plot. # GREEN ROTOR POINTS NORTH # BLUE ROTOR POINTS EAST # PARAMETERS # s defines the plot size in meters # swi controls flyer attitude plot; 1 = on, otherwise off. # INPUTS # 1 Center X position # 2 Center Y position # 3 Center Z position # 4 Yaw angle in rad # 5 Pitch angle in rad # 6 Roll angle in rad
[docs] def __init__(self, model, scale=[-2, 2, -2, 2, 10], flapscale=1, projection='ortho', **blockargs): """ Create a block that displays/animates a multi-rotor flying vehicle. :param model: A dictionary of vehicle geometric and inertial properties :type model: dict :param scale: dimensions of workspace: xmin, xmax, ymin, ymax, zmin, zmax, defaults to [-2,2,-2,2,10] :type scale: array_like, optional :param flapscale: exagerate flapping angle by this factor, defaults to 1 :type flapscale: float :param projection: 3D projection, one of: 'ortho' [default], 'perspective' :type projection: str :param blockargs: |BlockOptions| :type blockargs: dict :return: a MULTIROTORPLOT block :rtype: MultiRotorPlot instance **Block ports** :input y: a dictionary signal that includes the item: - ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]` - ``X`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]` - ``a1s`` - ``b1s`` .. figure:: ../../figs/multirotorplot.png :width: 500px :alt: example of generated graphic Example of quad-rotor display. Written by Pauline Pounds 2004 """ super().__init__(nin=1, **blockargs) self.type = 'quadrotorplot' self.model = model self.scale = scale self.nrotors = model['nrotors'] self.projection = projection self.flapscale = flapscale
def start(self, state): quad = self.model # vehicle dimensons d = quad['d']; # Hub displacement from COG r = quad['r']; # Rotor radius #C = np.zeros((3, self.nrotors)) ## WHERE USED? self.D = np.zeros((3,self.nrotors)) for i in range(0, self.nrotors): theta = i / self.nrotors * 2 * pi # Di Rotor hub displacements (1x3) # first rotor is on the x-axis, clockwise order looking down from above self.D[:,i] = np.r_[ quad['d'] * cos(theta), quad['d'] * sin(theta), quad['h']] #draw ground self.fig = plt.figure() # no axes in the figure, create a 3D axes self.ax = self.fig.add_subplot(111, projection='3d', proj_type=self.projection) # ax.set_aspect('equal') self.ax.set_xlabel('X') self.ax.set_ylabel('Y') self.ax.set_zlabel('-Z (height above ground)') self.panel = self.ax.text2D(0.05, 0.95, '', transform=self.ax.transAxes, fontsize=10, family='monospace', verticalalignment='top', bbox=dict(boxstyle='round', facecolor='white', edgecolor='black')) # TODO allow user to set maximum height of plot volume self.ax.set_xlim(self.scale[0], self.scale[1]) self.ax.set_ylim(self.scale[2], self.scale[3]) self.ax.set_zlim(0, self.scale[4]) # plot the ground boundaries and the big cross self.ax.plot([self.scale[0], self.scale[1]], [self.scale[2], self.scale[3]], [0, 0], 'b-') self.ax.plot([self.scale[0], self.scale[1]], [self.scale[3], self.scale[2]], [0, 0], 'b-') self.ax.grid(True) self.shadow, = self.ax.plot([0, 0], [0, 0], 'k--') self.groundmark, = self.ax.plot([0], [0], [0], 'kx') self.arm = [] self.disk = [] for i in range(0, self.nrotors): h, = self.ax.plot([0], [0], [0]) self.arm.append(h) if i == 0: color = 'b-' else: color = 'g-' h, = self.ax.plot([0], [0], [0], color) self.disk.append(h) self.a1s = np.zeros((self.nrotors,)) self.b1s = np.zeros((self.nrotors,)) def step(self, state): def plot3(h, x, y, z): h.set_data_3d(x, y, z) # h.set_data(x, y) # h.set_3d_properties(np.r_[z]) # READ STATE z = self.inputs[0]['x'][0:3] n = self.inputs[0]['x'][3:6] # TODO, check input dimensions, 12 or 12+2N, deal with flapping a1s = self.inputs[0]['a1s'] b1s = self.inputs[0]['b1s'] quad = self.model # vehicle dimensons d = quad['d'] # Hub displacement from COG r = quad['r'] # Rotor radius # PREPROCESS ROTATION MATRIX phi, the, psi = n # Euler angles # BBF > Inertial rotation matrix R = np.array([ [cos(the) * cos(phi), sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)], [cos(the) * sin(phi), sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi), cos(psi) * sin(the) * sin(phi) - sin(psi)* cos(phi)], [-sin(the), sin(psi)*cos(the), cos(psi) * cos(the)] ]) # Manual Construction #Q3 = [cos(psi) -sin(psi) 0;sin(psi) cos(psi) 0;0 0 1]; %Rotation mappings #Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; #Q1 = [1 0 0;0 cos(phi) -sin(phi);0 sin(phi) cos(phi)]; #R = Q3*Q2*Q1; %Rotation matrix # CALCULATE FLYER TIP POSITONS USING COORDINATE FRAME ROTATION F = np.array([ [1, 0, 0], [0, -1, 0], [0, 0, -1] ]) # Draw flyer rotors theta = np.linspace(0, 2 * pi, 20) circle = np.zeros((3, 20)) for j, t in enumerate(theta): circle[:,j] = np.r_[r * sin(t), r * cos(t), 0] hub = np.zeros((3, self.nrotors)) tippath = np.zeros((3, 20, self.nrotors)) for i in range(0, self.nrotors): hub[:,i] = F @ (z + R @ self.D[:,i]) # points in the inertial frame q = self.flapscale # Flapping angle scaling for output display - makes it easier to see what flapping is occurring # Rotor -> Plot frame Rr = np.array([ [cos(q * a1s[i]), sin(q * b1s[i]) * sin(q * a1s[i]), cos(q * b1s[i]) * sin(q * a1s[i])], [0, cos(q * b1s[i]), -sin(q*b1s[i])], [-sin(q * a1s[i]), sin(q * b1s[i]) * cos(q * a1s[i]), cos(q * b1s[i]) * cos(q * a1s[i])] ]) tippath[:,:,i] = F @ R @ Rr @ circle plot3(self.disk[i], hub[0,i] + tippath[0,:,i], hub[1,i] + tippath[1,:,i], hub[2,i] + tippath[2,:,i]) # Draw flyer hub0 = F @ z # centre of vehicle for i in range(0, self.nrotors): # line from hub to centre plot3([hub(1,N) hub(1,S)],[hub(2,N) hub(2,S)],[hub(3,N) hub(3,S)],'-b') plot3(self.arm[i], [hub[0,i], hub0[0]], [hub[1,i], hub0[1]], [hub[2,i], hub0[2]]) # plot a circle at the hub itself #plot3([hub(1,i)],[hub(2,i)],[hub(3,i)],'o') # plot the vehicle's centroid on the ground plane plot3(self.shadow, [z[0], 0], [-z[1], 0], [0, 0]) plot3(self.groundmark, z[0], -z[1], 0) textstr = f"t={state.t: .2f}\nh={z[2]: .2f}\nγ={n[0]: .2f}" self.panel.set_text(textstr) super().step(state=state)