您的位置: 首页  >  文章  >  四旋翼无人机

四旋翼无人机

分类: 文章 • 2024-02-12 22:59:40

四旋翼无人机

四旋翼无人机的动力学模型

为了建立起能够描述无人机的物理和运动特性的方程，需要定义建模时的坐标系。定义两种坐标系：固定坐标系（惯性坐标系）{A}和无人机的机身坐标系{B}。使用欧拉角ξ=(ϕ,θ,ψ)T\xi {\rm{ = }}{\left( {\phi, \theta ,\psi } \right)^T}表示在机身坐标系中无人机绕各个轴转动的角度，P=(x,y,z)TP={\left( {x, y ,z } \right)^T}表示无人机重心坐标。无人机为"X"型，无人机质量为mm，机臂长度为ll，绕无人机三个轴转动的转动惯量分别为Ixx,Iyy,IzzI_{xx},I_{yy},I_{zz}。无人机结构图如下
四旋翼无人机
四个旋翼的升力分别为F1,F2,F3,F4F_1,F_2,F_3,F_4，都沿着无人机机身zz轴方向。总升力u1=F1+F2+F3+F4u_1=F_1 + F_2 + F_3 + F_4, 作用于xx轴的力矩u2=l2(−F1−F2+F3+F4)u_2=\dfrac{l}{{\sqrt 2 }}( - F_1 - F_2 + F_3 + F_4), 作用于yy轴的力矩u3=l2(−F1+F2+F3−F4)u_3=\dfrac{l}{{\sqrt 2 }}( - F_1 +F_2 + F_3 - F_4), 作用于zz轴的力矩（扭矩） u4=−b(F1+F2−F3+F4)u_4= -b( F_1 + F_2 - F_3 + F_4), 其中bb 是力到扭矩的系数。根据牛顿-欧拉方程可得无人机运动学方程
IΩ˙+Ω×IΩ=NI\dot \Omega + \Omega \times I\Omega = N其中I是无人机惯性张量矩阵，认为无人机机身对称，则I=diag(Ixx,Iyy,Izz)I = diag(I_{xx},I_{yy},I_{zz})，Ω\Omega表示无人机绕各个轴转动的角速度Ω=(ϕ˙,θ˙,ψ˙)T\Omega=({\dot \phi, \dot \theta ,\dot \psi } )^T，NN表示作用在无人机上的合力矩，N=(u2，u3，u4)TN=(u_2，u_3，u_4)^T。展开得
ϕ¨=θ˙ψ˙Iyy−IzzIxx+u2Ixx \ddot \phi = \dot \theta \dot \psi \dfrac{{{I_{yy}} - {I_{zz}}}}{{{I_{xx}}}} + \dfrac{u_2}{{{I_{xx}}}} θ¨=ϕ˙ψ˙Izz−IxxIyy+u3Iyy \ddot \theta = \dot \phi \dot \psi \dfrac{{{I_{zz}} - {I_{xx}}}}{{{I_{yy}}}} + \dfrac{u_3}{{{I_{yy}}}} ψ¨=θ˙ϕ˙Ixx−IyyIzz+u4Izz \ddot \psi = \dot \theta \dot \phi \dfrac{{{I_{xx}} - {I_{yy}}}}{{{I_{zz}}}} + \dfrac{u_4}{{{I_{zz}}}} 由牛二定律
ma=BARzyxF ma=_B^A{R_{zyx}} F其中a=(x¨，y¨，z¨)Ta=(\ddot x，\ddot y，\ddot z)^T，F=(0，0，u1)TF=(0，0，u_1)^T。BARzyx_B^A{R_{zyx}}的计算在后面。展开得
x¨=u1(cos⁡ϕsin⁡θcos⁡ψ+sin⁡ϕsin⁡ψ)m \ddot x = \dfrac{{u_1(\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi )}}{m} y¨=u1(cos⁡ϕsin⁡θsin⁡ψ−sin⁡ϕcos⁡ψ)m \ddot y = \dfrac{{u_1(\cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi )}}{m} z¨=u1cos⁡ϕcos⁡θm−g \ddot z = \dfrac{{u_1\cos \phi \cos \theta }}{m} - g

四旋翼无人机的姿态表示

Z-Y-X 欧拉角

先将{B}坐标系绕{B}的z轴旋转ψ\psi角得到{C}坐标系，再将{C}坐标系绕{C}的y轴旋转θ\theta角得到{D}坐标系，再将{D}坐标系绕{D}的x轴旋转ϕ\phi角得到{A}坐标系，那么{B}坐标系变换到{A}坐标系的旋转矩阵为BARzyx=BCRz⋅CDRy⋅DARx_B^A{R_{zyx}} = _B^C{R_z} \cdot _C^D{R_y} \cdot _D^A{R_x}

四旋翼无人机
四旋翼无人机
容易得到(BARzyx)−1=ABRzyx=(BARzyx)T{\left( {_B^A{R_{zyx}}} \right)^{ - 1}} = _A^B{R_{zyx}} = {\left( {_B^A{R_{zyx}}} \right)^T}{B}坐标系中向量Bp=(x0,y0,z0)^Bp= \left({{x_0},{y_0},{z_0}} \right)在{A}坐标系中表示为·Ap=(x1,y1,z1)^Ap= \left({{x_1},{y_1},{z_1}} \right)，那么
Ap=BARzyxBp^Ap=_B^A{R_{zyx}} ^Bp

欧拉角微分方程

对上面的BARzyx_B^A{R_{zyx}}求微分，得
四旋翼无人机
具体过程参见yangoming的博客(http://blog.sina.com.cn/s/blog_40edfdc90102wazm.html)
当θ=π/2\theta=\pi/2时会遇到万向节死锁，转动失去一个*度，欧拉角微分方程无法表示，所以四元数微分方程而不用欧拉角微分方程。

四元数

四元数与虚数

我们都知道虚数形式为q=w+x⋅i⃗q = w + x \cdot\vec i可以写为q=w+x⋅i⃗+0⋅j⃗+0⋅k⃗q = w + x \cdot \vec i + 0\cdot\vec j + 0\cdot\vec k 单位虚数q^=eiθ=cos⁡θ+i⃗⋅sinθ=cos⁡θ+sin⁡θ(1∗i⃗+0∗j⃗+0∗k⃗)\hat q= {e^{i\theta }}=\cos \theta + \vec i\cdot sin \theta=\cos \theta + \sin \theta \left( {1*\vec i + 0*\vec j + 0*\vec k} \right)p^q\hat pq可以表示一个向量pp绕轴（或向量）(1,0,0)(1,0,0)旋转θ\theta得到的向量。
四元数是虚数的扩展，一个四元数可以表示为 q=w+xi⃗+yj⃗+zk⃗q = w + x\vec i + y\vec j + z\vec k 单位四元数q^=eθ2(xi⃗+yj⃗+zk⃗)=cos⁡θ2+(xi⃗+yj⃗+zk⃗)sin⁡θ2{\hat q=e^{\frac{\theta }{2}\left( {x\vec i + y\vec j + z\vec k} \right)}} = \cos {\frac{\theta }{2}} + \left( {x\vec i + y\vec j + z\vec k} \right)\sin {\frac{\theta }{2}} 且q^pq^∗\hat qp{{\hat q}^ * }可以表示一个向量pp绕轴（或向量）(x,y,z)( x,y,z)旋转θ\theta得到的向量。其中x2+y2+z2=1{x^2} + {y^2} + {z^2} = 1，q^∗{{\hat q}^ * }是q^\hat q的共轭。

四元数乘法

简便起见四元数可以写为q=w+v⃗⋅x⃗=(w,v⃗)q = w + \vec v \cdot \vec x=(w,\vec v)其中ww为标量，vv为向量,v⃗=(x,y,z),x⃗=(i⃗,j⃗,k⃗),\vec v = \left( {x,y,z} \right),\vec x = \left( {\vec i,\vec j,\vec k} \right)
两个四元数的乘法按多项式乘法进行,i⃗j⃗=k⃗,j⃗k⃗=i⃗,k⃗i⃗=j⃗\vec i\vec j = \vec k,\vec j\vec k = \vec i,\vec k\vec i{\rm{ = }}\vec jq1=(w1,v⃗1){q_1} = \left( {{w_1},{\vec v_1}} \right),q2=(w2,v⃗2){q_2} = \left( {{w_2},{\vec v_2}} \right)，q1q2{q_1q_2}化简得w1w2−v⃗1⋅v⃗1,v⃗1×v⃗2+w1v⃗2+w2v⃗1{{w_1w_2-\vec v_1 \cdot \vec v_1},{\vec v_1 \times \vec v_2+w_1\vec v_2+w_2\vec v_1}}假设单位四元数q^=q0+q1i⃗+q2j⃗+q3k⃗\hat q = q_0+ q_1\vec i + q_2\vec j + q_3\vec k可以表示为 q^=[q0,q1,q2,q3]\hat q = \left[ {q_0},{q_1},{q_2},{q_3}\right] 向量p=(x0,y0,z0)p= \left({{x_0},{y_0},{z_0}} \right)的四元数p=[0,x0,y0,z0]p= \left[ 0,{{x_0},{y_0},{z_0}} \right]。
p′=q^pq^∗p' =\hat qp{{\hat q}^ * }得到四元数p′=[0,x1,y1,z1]p'= \left[ 0,{{x_1},{y_1},{z_1}} \right]。对应的向量p′=(x1,y1,z1)p' = \left( {{x_1},{y_1},{z_1}} \right)。四旋翼无人机
对比欧拉角旋转矩阵，可以知道四旋翼无人机

四元数微分方程

假设单位四元数q=cos⁡θ2+n⃗sin⁡θ2q = \cos \frac{\theta }{2} + \vec n\sin \frac{\theta }{2}表示惯性坐标系{A}绕向量n⃗\vec n旋转θ\theta角得到机体坐标系{B}，那么θ˙=Aω⃗\dot \theta=^A\vec\omega。对两边微分，得q˙=−12θ˙sin⁡θ2+12n⃗θ˙cos⁡θ2+dn⃗dtsin⁡θ2\dot q = - \frac{1}{2}\dot \theta \sin \frac{\theta }{2} + \frac{1}{2}\vec n\dot \theta \cos \frac{\theta }{2} + \frac{{d\vec n}}{{dt}}\sin \frac{\theta }{2}
其中dn⃗dt=ϖ×n⃗=θ˙n⃗×n⃗=0\frac{{d\vec n}}{{dt}} = \varpi \times \vec n = \dot \theta \vec n \times \vec n = 0 所以
q˙=−12θ˙sin⁡θ2+12n⃗θ˙cos⁡θ2=n⃗θ˙2(cos⁡θ2+n⃗sin⁡θ2)=12Aω⃗q\dot q = - \frac{1}{2}\dot \theta \sin \frac{\theta }{2} + \frac{1}{2}\vec n\dot \theta \cos \frac{\theta }{2} = \vec n\frac{{\dot \theta }}{2}\left( {\cos \frac{\theta }{2} + \vec n\sin \frac{\theta }{2}} \right) = \frac{1}{2} {^A\vec\omega} qBω⃗^B\vec\omega是机体角速度，Bω⃗=qAω⃗q∗^B\vec\omega {\rm{ = }}{q^A}\vec\omega {q^ * }，所以q˙=12qBω⃗\dot q = \frac{1}{2} q {^B\vec\omega}
q˙=(q0+q1i⃗+q2j⃗+q3k⃗)(ωxi⃗+ωyj⃗+ωzk⃗)\dot q =(q_0 + q_1\vec i + q_2\vec j + q_3\vec k)({\omega _x}\vec i + {\omega _y}\vec j + {\omega _z}\vec k) 整理得
四旋翼无人机
四元数微分方程可用来更新四元数，进而更新旋转矩阵。

四旋翼无人机的姿态解算

姿态解算就是求欧拉角ξ=(ϕ,θ,ψ)T\xi {\rm{ = }}{\left( {\phi, \theta ,\psi } \right)^T}，等价于求旋转矩阵。
无人机机体携带的加速度计测加速度向量Ba⃗^B\vec a，陀螺仪测量三个轴的角速度Bω⃗^B\vec\omega，如果有磁力计可以测地磁场强度向量BH⃗^B\vec H。
先不考虑测量数据的误差，仅凭角速度更新旋转矩阵。使用一阶龙格库塔法更新四元数
qt+Δt=qt+Δtq˙t{q_{t + \Delta t}} = {q_t} + \Delta t{{\dot q}_t}然后四元数单位化，得到四元数表示的旋转矩阵。

传感器的特性

传感器但是有误差的，陀螺仪高频特性好，某一时刻测得的角速度值比较精准，但积分会有很大的漂移；而加速度计和磁力计低频特性好，总体趋势比较准。姿态解算需要利用静态性能好的加速度计和磁力计取补偿动态性能好的陀螺仪，得到不漂并且高速的姿态跟踪算法。
传感器使用前需要校正。陀螺仪校正是测系统初始时一段时间内的测量值的平均值作为偏移值，后面的测量值减去这个偏移值使用；加速度计校正，加速度计测量值分布在一个球面上，用球面方程拟合初始一段时间的数据得到球面半径即为重力加速度的值（单位是g）;地磁场一般情况下大小只有0.5高斯，很容易受到铁钴镍金属和其他磁场的干扰，一般认为干扰磁场是一个恒定的向量。校准方法有平面校准法和立体8字校准法。

加速度补偿陀螺仪

重力加速度g⃗=gz⃗\vec g = g\vec z,归一化g^=z^=(0,0,1)T\hat g = \hat z=(0,0,1)^T，旋转至机体坐标系得Bg^^B\hat g,Bg^=ABRzyxAg^=(−sin⁡θ,sin⁡ϕcos⁡θ,cos⁡ϕcos⁡θ)T^B\hat g = _A^B{R_{zyx}}^A\hat g={\left( { - \sin \theta ,\sin \phi \cos \theta ,\cos \phi \cos \theta } \right)^T}（可以看到在载体本身无加速度的情况下加速度计的测量值和角ψ\psi无关，所以仅凭加速度计无法完全补偿陀螺仪。）
加速度计测得的加速度Ba⃗^B\vec a做归一化Ba^^B\hat a，向量积得出姿态误差
Verror=Ba^×Bg^{V_{error}}{ = ^B}\hat a{ \times ^B}\hat g向量积误差是指将带有误差的加速度计向量Ba^^B\hat a转动至与准确的加速度计向量Bg^^B\hat g重合需要绕什么轴转多少角度。如果完全按照向量积误差转过去，就是完全信任加速度计，如果一点也不转就是完全信任陀螺仪。如果把这个误差乘以一个系数加到角速度上去就是互补滤波。也可以用Mahony的PI滤波,VI+=ki∗Verror{V_{I}} += k_i*V_{error}Gyro+=kp∗Verror+VIGyro += k_p*V_{error}+{V_{I}}kpk_p是加速度权重，越大则向加速度测量值的收敛越快。GyroGyro就是得到的修正后的角速度值。

磁力计补偿陀螺仪

加速度计的测量值和角ψ\psi无关，所以仅凭加速度计无法完全补偿陀螺仪。磁力计直接测得的是地磁场向量BH^BH，对一个固定地点来说地磁场一般是一定的。这个向量可以分解为两个与当地水平面平行的分量和一个与当地水平面垂直的分量。对于水平方向的两个分量，他们的向量和方向总是指向磁北的。航向角ψ\psi是就是当前方向和磁北的夹角。
四旋翼无人机AH=BARyxBH^AH = _B^A{R_{yx}}^BH如果完全信赖磁力计ψ=atan2(AHy,AHx)\psi= atan2(^AH_y,^AH_x)让惯性坐标系的xx轴指向磁北，那么AH=(AHx,0,AHz)^AH =(^AH_x,0,^AH_z)。BH=(BHx,BHy,BHz)^BH =(^BH_x,^BH_y,^BH_z),AH′=BARzyxBH=(hx,hy,hz)^AH' = _B^A{R_{zyx}}^BH=(h_x,h_y,h_z)AHx=hx2+hy2,AHz=hz^A{H_x} = \sqrt {h_x^2 + h_y^2}, ^A{H_z} = {h_z} 将AH^AH旋转至机身坐标系得BH′=ABRzyxAH^BH '= _A^B{R_{zyx}}^AH，BH′^BH '和BH^BH做向量积，向量积误差仍然通过Mahony的PI滤波加到角速度上，这样就完成了一次地磁计的补偿。

四旋翼无人机的控制

PID控制

PID（比例-积分-微分）控制方法是一种简单有效的线性控制方法，广泛应用于工业控制。PID控制是一种单入单出系统的反馈控制方法，输入控制系统目标状态值和实际状态的差值ee，得到系统的控制输入uu
u=kPe+kI∫t0tedt+kDdedtu = {k_P}e + {k_I}\int_{{t_0}}^t {edt} + {k_D}\frac{{de}}{{dt}}
无人机系统是一个多输入多输出的欠驱动强耦合的非线性系统，控制输入为四个旋翼的升力分别为F1,F2,F3,F4F_1,F_2,F_3,F_4，输出为六个状态x,y,z,ϕ,θ,ψ{x, y ,z,\phi, \theta ,\psi }。不能设计6个独立的PID控制器，因为系统的控制输入只有4个。观察无人机系统的动力学方程ϕ¨=θ˙ψ˙Iyy−IzzIxx+u2Ixx \ddot \phi = \dot \theta \dot \psi \dfrac{{{I_{yy}} - {I_{zz}}}}{{{I_{xx}}}} + \dfrac{u_2}{{{I_{xx}}}} θ¨=ϕ˙ψ˙Izz−IxxIyy+u3Iyy \ddot \theta = \dot \phi \dot \psi \dfrac{{{I_{zz}} - {I_{xx}}}}{{{I_{yy}}}} + \dfrac{u_3}{{{I_{yy}}}} ψ¨=θ˙ϕ˙Ixx−IyyIzz+u4Izz \ddot \psi = \dot \theta \dot \phi \dfrac{{{I_{xx}} - {I_{yy}}}}{{{I_{zz}}}} + \dfrac{u_4}{{{I_{zz}}}} x¨=u1(cos⁡ϕsin⁡θcos⁡ψ+sin⁡ϕsin⁡ψ)m \ddot x = \dfrac{{u_1(\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi )}}{m} y¨=u1(cos⁡ϕsin⁡θsin⁡ψ−sin⁡ϕcos⁡ψ)m \ddot y = \dfrac{{u_1(\cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi )}}{m} z¨=u1cos⁡ϕcos⁡θm−g \ddot z = \dfrac{{u_1\cos \phi \cos \theta }}{m} - g 1. 因为四旋翼在飞行过程中偏角ϕ\phi和θ\theta一般很小，所以近似认为zz的状态只与u1u_1有关，所以先用一个独立的PID控制这个通道得到u1u_1。
2.ψ\psi也可近似认为只与u4u_4有关，所以用一个独立的PID控制这个通道得到u4u_4。
3.对xx和yy通道，xx和yy没有直接和控制量u2u_2和u3u_3产生关系，但是xx和yy与ϕ\phi和θ\theta有关，ϕ\phi和θ\theta和u2u_2和u3u_3有关，所以对xx和yy通道分别使用嵌套双层PID控制。如xx通道外层PID控制器产生一个虚拟控制律θdesired\theta_{desired}，作为内层PID控制器的参考输入，内层PID控制器的输入为θdesired−θ\theta_{desired}-\theta，产生u3u_3，yy通道类似产生u4u_4。
PID控制是单输入单输出控制，应用于四旋翼无人机控制是把无人机控制分成了四个独立通道，分别控制，忽略了各个通道的耦合关系。

相关推荐