旋翼无人机的建模、规划与控制

旋翼无人机的建模、规划与控制目录TOC\h\h第1章位置、姿态描述与坐标系变换\h1.1概述\h1.1.1右手定则\h1.1.2地球表面惯性坐标系与机体坐标系\h1.1.3欧拉角\h1.2位置、姿态\h1.2.1位置描述（移动）\h1.2.2姿态描述（旋转）\h1.2.3坐标系绕坐标轴转动\h1.3坐标系变换\h1.3.1旋转矩阵的用法\h1.3.2连续转动\h1.3.3齐次变换矩阵\h1.3.4刚体状态的表达\h1.4姿态的其他描述方法\h1.4.1X-Y-Z固定角坐标系\h1.4.2等效轴角坐标系表示法\h1.4.3欧拉参数法\h第2章旋翼无人机的建模分析\h2.1旋翼无人机系统的坐标系介绍\h2.2旋翼无人机的运动分析\h2.2.1平移运动分析\h2.2.2旋转运动分析\h2.3旋翼的陀螺效应\h2.4旋翼的空气动力学效应\h2.5空气阻力\h2.6旋翼无人机模型的建立\h2.6.1受力在机体系和导航系之间的转换\h2.6.2欧拉角速度与机体角速度的转换\h2.6.3旋翼无人机的动力学模型\h第3章旋翼无人机的运动信息估计\h3.1旋翼无人机系统中传感器简介\h3.1.1九轴惯性测量单元\h3.1.2GPS\h3.1.3光流\h3.1.4气压计\h3.2系统可观性分析\h3.3姿态估计\h3.3.1互补滤波\h3.3.2梯度下降法\h3.3.3卡尔曼滤波\h3.4位置估计\h3.4.1加速度计积分\h3.4.2组合导航（GPS与IMU）\h3.4.3光流（室内）定位\h第4章旋翼无人机视觉定位方法及系统\h4.1视觉定位方法简介\h4.2相机标定\h4.3Kinect外部视觉定位\h4.4单目视觉系统\h4.5双目视觉系统\h4.5.1双目标定\h4.5.2双目定位\h第5章旋翼无人机的路径规划\h5.1旋翼无人机航迹规划的基本原理\h5.1.1旋翼无人机航迹规划的定义\h5.1.2旋翼无人机航迹规划系统\h5.1.3旋翼无人机航迹规划的应用原理\h5.2旋翼无人机航迹规划方法综述\h5.2.1环境建模方法\h5.2.2航迹规划搜索算法\h5.3SLAM算法\h5.3.1SLAM的一般过程\h5.3.2SLAM问题的一般模型\h5.3.3基于EKF的SLAM研究\h5.4A*算法\h5.4.1A*算法搜索步骤\h5.4.2稀疏A*算法\h5.5人工势场法\h5.5.1基本人工势场法的路径规划\h5.5.2三维环境下使路径平滑的人工势场法\h5.5.3基于人工势场法的路径规划仿真\h5.6生物启发神经动力学模型算法\h5.7航迹规划的融合算法\h5.7.1稀疏A*与神经动力学融合算法\h5.7.2A*与人工势场融合算法\h第6章旋翼无人机控制器的设计\h6.1旋翼无人机控制框架\h6.1.1位置控制\h6.1.2姿态控制\h6.1.3控制分配\h6.1.4电机控制\h6.2常用的控制算法及控制器的实现\h6.2.1PID控制\h6.2.2反步法控制\h6.2.3滑模变结构控制\h6.2.4神经网络控制\h第7章机载操作臂\h7.1机载操作臂简介\h7.1.1背景\h7.1.2机载操作臂的机构与控制\h7.2机载操作臂运动学\h7.2.1连杆坐标系的描述和规定\h7.2.2关节空间和笛卡尔空间\h7.2.3机载操作臂运动学\h7.2.4机载操作臂逆运动学\h7.3速度和静力\h7.3.1刚体的线速度和角速度\h7.3.2连杆间的速度传递\h7.3.3雅可比矩阵\h7.3.4作用在操作臂上的静力\h7.3.5速度和静力的笛卡尔变换\h7.4机载操作臂动力学\h7.4.1加速度\h7.4.2牛顿-欧拉迭代动力学方程\h7.4.3迭代形式与封闭形式的动力学方程\h7.4.4机载操作臂动力学的拉格朗日公式\h7.5机载操作臂的末端轨迹生成\h7.5.1关节空间规划方法\h7.5.2笛卡尔空间规划方法\h7.5.3路径的实时生成\h7.5.4无碰撞路径规划\h7.6机载操作臂的控制\h7.6.1单关节的建模和控制\h7.6.2多输入多输出控制系统\h7.6.3李雅普诺夫稳定性分析\h7.6.4基于笛卡尔空间的控制系统\h第8章带臂旋翼无人机\h8.1带臂旋翼无人机简介\h8.1.1概述\h8.1.2带臂旋翼无人机的结构\h8.1.3带臂旋翼无人机的特点\h8.2带臂旋翼无人机的联合建模\h8.2.1带臂旋翼无人机的运动学模型\h8.2.2带臂旋翼无人机的动力学模型\h8.2.3带臂旋翼无人机的协调控制\h8.3带臂旋翼无人机的视觉伺服控制\h8.3.1视觉伺服控制\h8.3.2基于位置的视觉伺服控制\h8.3.3基于图像的视觉伺服控制\h8.3.4基于视觉伺服控制的带臂旋翼无人机\h8.4带臂旋翼无人机的应用第1章位置、姿态描述与坐标系变换1.1概述1.1.1右手定则右手的拇指指向x轴的正方向，食指指向y轴的正方向，中指所指示的方向即是z轴的正方向。进一步，如图1.1所示，要确定轴的正旋转方向，用右手的大拇指指向轴的正方向，弯曲手指，那么弯曲手指所指示的方向即是轴的正旋转方向。本章采用的坐标系和后面定义的角度正方向都是沿用右手定则。图1.1右手定则下的坐标轴和旋转正方向1.1.2地球表面惯性坐标系与机体坐标系地球表面惯性坐标系（简称惯性坐标系）用于研究多旋翼飞行器（多旋翼飞行器指四旋翼飞行器以上的飞行器，本文主要讨论四旋翼）相对于地面的运动状态，确定机体的空间位置坐标（见图1.2）。它忽略了地球曲率，即假设地球表面是一个平面。在地面上选一点作为多旋翼飞行器起飞位置，z轴垂直于地面向下，并使x轴或y轴在水平面内指向某一方向，则惯性坐标系确定。然后，按右手定则确定x轴、y轴。机体坐标系，其原点取在多旋翼飞行器的重心上，坐标系与多旋翼飞行器固连（见图1.2）。z轴在多旋翼飞行器对称平面内指向机头（机头方向与多旋翼飞行器十字形或X字形相关）。z轴在飞行器对称平面内，垂直轴向下。然后，按右手定则确定x轴，y轴。其中定义三个单位向量：图1.2地球表面惯性坐标系与机体坐标系在地球表面惯性坐标系中，沿着轴xe，ye，ze（右下标e表示earth）的单位向量可以表示为｛e1，e2，e3｝。在机体坐标系下，沿着xb，yb，zb（右下标b表示body）轴的单位向量满足bb1＝e1，bb2＝e2，bb3＝e3。（左上标b表示向量在机体坐标系）。在地球表面惯性坐标系中，沿着轴xb，yb，zb轴的单位向量表示为｛eb1，eb2，eb3｝（左上标e表示向量在地球表面惯性坐标系）。1.1.3欧拉角欧拉角的定义（见图1.3）：图1.3欧拉角直观表示示意图机体坐标系与地面惯性坐标系之间的夹角就是飞行器的姿态角，又称欧拉角（见图1.4）。俯仰角θ：机体轴与地平面（水平面）之间的夹角，飞行器抬头为正。偏航角（方位角）ψ：机体轴在水平面上的投影与地轴之间的夹角，以机头左偏为正。滚转角（倾斜角）φ：飞行器对称面绕机体轴转过的角度，右滚为正（见图1.5）。图1.4飞机欧拉角示意图图1.5正的俯仰角θ，负的滚转角φ，正的偏航角Ψ1.2位置、姿态1.2.1位置描述（移动）概念：位置描述指以向量P来描述｛B｝的原点相对于｛W｝的状态。例如图1.6用向量P描述坐标系｛B｝原相对坐标系｛W｝其中｛W｝为世界坐标系，｛B｝为机体坐标系。两个空间坐标系的位置关系：1.2.2姿态描述（旋转）概念：姿态描述表示两个空间坐标系坐标轴的方向关系（见图1.7）。图1.7坐标系姿态描述图1.8姿态的求解解：（见图1.8）因此，｛B｝相对于｛A｝的姿态：旋转矩阵的性质（见图1.9）：图1.9旋转矩阵性质正交矩阵的性质：AAT＝ATA＝I。因此旋转矩阵为3×3的正交矩阵，同样旋转矩阵R满足正交矩阵的性质。3）坐标系矩阵R有9个数字，但上列两个条件置入了6个限制条件，所以R只有3个自由度，与空间中转动具有3个自由度相符。1.2.3坐标系绕坐标轴转动其中θ为旋转角度。图1.10坐标轴旋转1.3坐标系变换1.3.1旋转矩阵的用法旋转矩阵R（见图1.11）有三种用法（见图1.12）。1）描述一个坐标系相对于另一个坐标系的姿态。图1.11旋转矩阵2）对于一个点P，由其在坐标系｛B｝中的表达转换到坐标系｛A｝中的表达。其中坐标系｛A｝与坐标系｛B｝仅有相对转动。AP＝（见图1.12）推导证明：在初始坐标系｛B｝中，点P的坐标：在新坐标系｛A｝中，点P的坐标：图1.12｛B｝坐标系转换到｛A｝坐标结果如图1.13所示。3）点P在绕着坐标系的某一坐标轴旋转，得到新的点P′（见图1.14）。AP′＝R（θ）AP图1.13上视图验证图1.14点P在同一坐标中转动图1.15AP求解1.3.2连续转动空间中的旋转矩阵有3个自由度（见图1.16），要表达一般的旋转矩阵，可拆解成3次旋转角度，以对应到空间中的3个自由度。Z-Y-X欧拉角：图1.16旋转矩阵3个自由度1）将坐标系｛A｝与坐标系｛B｝的原点重合。2）将坐标系｛A｝绕着轴旋转α角度，并把新的坐标系记为｛B′｝。3）将坐标系｛B′｝绕着轴旋转β角度，并把新的坐标系记为｛B″｝。4）将坐标系｛B″｝绕着轴旋转γ角度，新的坐标系记为｛B‴｝，｛B‴｝与｛B｝重合。如果把这些旋转看成是坐标系的描述，就可写出：根据Z-Y-X欧拉角的定义，每个旋转矩阵都是绕着自身坐标系旋转的：例1-4：以欧拉角方法旋转，先对X轴旋转60°，后对Y轴旋转30°，和先对Y轴旋转30°，后对X轴旋转60°各自的相同么？解：①先对X轴旋转60°，后对Y轴旋转30°：②先对Y轴旋转30°，后对X轴旋转60°：因此可发现两个旋转矩阵并不相同。这里要注意坐标轴旋转的次序，不同次序组合的欧拉角，将得到不同的旋转矩阵。旋转矩阵反推旋转角度：若β≠±90°，可得到：Z-Y-Z欧拉角：1.3.3齐次变换矩阵对于一个点P，由其在坐标系｛B｝中的表达转换到坐标系｛A｝中的表达。例1-7：则AP为多少？图1.17求解AP示意图解：（参考图1.17）将点P1在同一坐标系中进行转动和移动到P1－2位置（见图1.18），那么P1与P2之间可表达为：图1.18点P在坐标系中的移动和转动图齐次变换矩阵的连续运算，见图1.19。图1.19齐次变换矩阵的连续运算连续运算，求未知的相对关系。求矩阵T的反矩阵。解：（参考图1.20）图1.20求解矩阵T的反矩阵1.3.4刚体状态的表达刚体的状态由建立在刚体上的坐标系来表示，坐标系原点常建立在重心上。而刚体状态的变化，由坐标系的移动与旋转变化来表示（见图1.21）。移动：由机体坐标系｛B｝的原点位置判定。旋转：由机体坐标系｛B｝的姿态判定。图1.21刚体状态表达为了便于连续运算，通过一个特殊形式的齐次变换，将移动和旋转整合起来。即为齐次变换矩阵。齐次变换矩阵有三种用法：1）描述一个坐标系相对于另一个坐标系的空间状态[见图1.22（a）]。2）对于一个点P，由其在坐标系｛B｝中的表达转换到坐标系｛A｝中的表达[见图1.22（b）]。3）将点P在同一坐标系中进行移动和转动[见图1.22（c）]。图1.22齐次变换矩阵1.4姿态的其他描述方法1.4.1X-Y-Z固定角坐标系X-Y-Z固定角坐标系，为描述坐标系的另一种方式（见图1.23）。1）将坐标系｛A｝与坐标系｛B｝的原点重合。图1.23描述坐标系的三种不同方式不同于前面介绍的欧拉角，这里每个旋转都是绕着固定参考坐标系｛A｝的轴。这里可直接推导等价旋转矩阵：注意这个结果与以Z-Y-X欧拉角旋转得到的结果完全相同。可以总结，绕固定轴旋转3次的最终姿态和以相反顺序绕运动坐标轴旋转3次的最终姿态相同。因为表达式相同，所以X-Y-Z固定角的反算也与之前介绍过的Z-Y-X欧拉角的反算完全相同，这里不做赘述。1.4.2等效轴角坐标系表示法对于空间中两个任意姿态的坐标系，总可以在空间里找到某个轴，使其中一个坐标系绕该轴旋转一个角度就能与另一个坐标系姿态重合。此时所说的轴是一般方向的轴（不是之前所说的主轴）。式中vθ＝1－cosθ。上式将等效轴角坐标系表示转变成旋转矩阵表示。反算：对于给定的旋转矩阵，求与θ。若θ角在0°和180°之间（θ≠0°或180°），则：1.4.3欧拉参数法另一种姿态表示法是通过4个数值来表示的，称为欧拉参数。根据等效旋转轴＝（kx，ky，kz）T和等效旋转角θ得到欧拉参数如下：这4个参数不是独立的：这个关系式是恒定的。4个参数存在这一个限制条件，同样与空间姿态有3个自由度相符。四元数的4个参数，可以看作是四维空间中单位超球面上的一点。用欧拉参数组表示旋转矩阵Rε：用已知的旋转矩阵Rε得到对应的欧拉参数：这里注意：当θ角等于180°时，ε4＝0，式子将失去意义。欧拉参数法的应用：对刚体进行旋转时，如果对两个正交矩阵复合需要进行27次乘法和18次加法；而通过欧拉参数的形式只需要16次乘法和12次加法，这对嵌入式系统尤其重要。第2章旋翼无人机的建模分析2.1旋翼无人机系统的坐标系介绍在对飞行器进行姿态分析时，要依靠参考坐标系，因此首先建立两个参考坐标系：一个是机体坐标系Ob（xb，yb，zb），而另一个是地球表面惯性坐标系Oe（xe，ye，ze），对于定义的两个直角参考坐标系，它们的坐标轴都符合右手定则。如图2.1所示，机体坐标系的坐标原点Ob位于四旋翼飞行器的重心，机体坐标系的zb轴位于垂直于飞行器平面上的垂直线上。xb轴和飞行器的一根轴上的机架在同一条直线上，前方指向xb轴的正方向。yb轴则和飞行器上的另一根轴上的机架在同一条直线上，右方为xb轴正方向。地球表面惯性坐标系的原点Oe固定在地面的某一点，先让xe轴在水平面内指向某一方向，ze轴垂直于地面向下，根据右手定则可确定轴ye。图2.1地球表面惯性坐标系与机体坐标系2.2旋翼无人机的运动分析四旋翼飞行器通过调节4个电机转速来改变旋翼转速，实现升力的变化，从而控制飞行器的姿态和位置。四旋翼飞行器是一种6自由度的垂直升降机，虽然只有4个输入力，同时却有6个状态输出，所以它又是一种欠驱动系统。可沿x，y，z轴平移运动，以及沿着x，y，z轴旋转运动。基本运动状态分别是：升降运动、滚转运动、俯仰运动、偏航运动和水平运动，分别对应沿z轴平移、绕x轴旋转、绕y轴旋转、绕z轴旋转、沿x或y轴平移运动。2.2.1平移运动分析1）升降运动。如图2.2所示，升降运动是指飞行器沿z轴的正方向或负方向运动。同时等量地增大或减小四个电机的转速，即可实现增大或减小螺旋桨沿z轴方向的升力（如图2.2所示，分别为T1，T2，T3，T4）。当垂直方向的升力大于重力（见图2.2中的G）和阻力的和力时，即可实现沿z轴负方向运动；当重力大于垂直方向的升力和阻力的和力时，即可实现沿重力方向运动；当垂直方向的升力等于重力时，即可实现飞行器的定高飞行。因为4个电机的升力及转速是等量增减的，所以升降运动是独立的，不会影响其他维度的运动。经过简单分析可知，当电机转速改变量相同时，X字形结构的飞行器所产生的转动力矩大小大约是十字形结构飞行器的倍。所以X字形结构的无人机的机动性要好于十字形结构的无人机。图2.2升降运动2）水平运动。水平运动是指飞行器沿x轴或y轴方向运动。如图2.2升降运动所示的飞行器，由于阻力的存在，飞行器保持水平姿态实现水平运动是不可能的，必须依靠飞行器的升力在水平面上的分力提供动力克服阻力，才能实现飞行器的水平运动。因此必须先进行相应的滚转运动及俯仰运动方可实现水平运动，且进行水平运动时飞行器必须保持某一姿态才可克服阻力保持飞行。因此水平运动与俯仰运动及滚转运动是耦合的，飞行器只有4个独立的自由度。由于飞行器的水平运动与俯仰运动及滚转运动有关，俯仰运动及滚转运动的灵敏度直接影响到水平运动的灵敏度。2.2.2旋转运动分析1）滚转运动。滚转运动是指飞行器绕x轴转动，使飞行器在y轴方向产生角度变化。如图2.3所示，当同时等量增加y轴负方向的二号、三号两电机转速，等量减小y轴正方向一号、四号两电机转速，即可实现四个电机升力对y轴方向力矩大小的改变。由于x轴正方向力矩增大，飞行器将会绕x轴正方向转动，进而实现滚转运动。若要实现沿x轴负方向转动，只需等量增加一号、四号电机转速，等量减小二号、三号电机转速即可。由于增大的升力与减小的升力相同，因此升力的合力在z轴方向的大小不会改变，即滚转运动不会影响升降运动。但由于滚转运动使姿态角发生改变，升力的合力会在y轴方向上产生分力，进而发生沿y轴的水平运动，所以滚转运动与沿y轴的运动是耦合的。滚转运动必然伴随沿y轴的水平运动。图2.3滚转运动2）俯仰运动。同理，俯仰运动是指飞行器绕y轴转动，使飞行器在x轴方向产生角度变化。如图2.4所示，当同时等量增加x轴正方向一号、二号两电机转速，等量减小x轴负方向三号、四号两电机转速，即可实现四个电机升力对y轴力矩大小的改变。由于y轴正方向力矩增大，飞行器将会绕y轴正方向转动，进而实现俯仰运动。若要实现沿y轴负方向转动，只需等量增加三号、四号电机转速，等量减小一号、二号电机转速即可。与滚转运动情况相似，升力的合力在z轴方向的大小不会改变，俯仰运动不会影响升降运动。俯仰运动使姿态角发生改变，升力的合力会在x轴方向上产生分力，进而发生沿x轴的水平运动，所以俯仰运动与沿x轴的运动是耦合的。俯仰运动必然伴随沿x轴的水平运动。2.4俯仰运动3）偏航运动。偏航运动是指飞行器绕z轴方向转动，偏航运动可以改变飞行器机头方向。如图2.5所示，当等量增大对角一号、三号电机转速，等量减小对角二号、四号电机转速，即可实现飞行器在z轴正方向上角动量的不平衡，进而使飞行器发生绕z轴正方向的转动。同理，若要实现飞行器绕z轴负方向的转动只需等量增大二号、四号电机转速，等量减小一号、三号电机的转速即可。图2.5偏航运动在偏航运动中，由于4个升力的大小是等量增减的，故不会对z轴方向上升力的合力产生影响，即偏航运动与升降运动是独立的。偏航运动时倾斜姿态角未发生变化，不会在x轴及y轴上产生力矩，同样也不会在水平面产生分力，所以偏航运动与水平运动、滚转运动、俯仰运动都是相互独立的。2.3旋翼的陀螺效应陀螺效应是指旋转中的物体在旋转过程中保持其动量矩矢量方向不变的特性。这种特性有两个特点：定轴性和进动性。定轴性是指当旋转的物体没有受到外力矩作用时，其动量矢量在惯性坐标系中的方向维持与初始方向不变的性质。进动性是指当旋转的物体受到外力矩M作用时，不会像静止的物体沿着受力方向倾倒，而是使旋转的物体动量矩矢量H端点绕着转轴以一定角速度ω做进动运动，如图2.6所示。根据赖柴定理，对于旋转的物体，动量矩相当于给此物体施加了一个约束，其约束是一对力偶，称为陀螺力矩。这对力矩表明对于高速旋转的物体，在空间方向上必须有一外力矩克服其产生的陀螺力矩。由以上分析可知要想迫使力矩改变，陀螺效应产生有两个前提条件：①物体必须绕着一个极轴旋转（产生动量矩）；②物体必须有一个外力矩克服陀螺力矩，使其动量矩的方向发生改变。旋翼绕zb轴方向做高速转动时，若zb轴发生转动会形成阻抗力矩，该力矩称为旋翼的陀螺效应力矩。由陀螺力学可知，陀螺效应力矩的表达式如式（2-1）所示，其中J∑为电机与桨叶转动惯量的和，ω为机体坐标系的角速度，Ω为桨叶角速度。由式（2-1）可知，当机体坐标系发生转动时，会在z轴及z轴方向形成陀螺力矩。设机体坐标系的角速度为ω＝（pqr）T，则在机体坐标下4个桨叶的陀螺效应力矩可表示为式（2-2）的形式。图2.6刚体的进动2.4旋翼的空气动力学效应当飞行器具有前飞的速度时，旋翼不仅会受到沿着构造旋转轴的升力F和反扭矩Q的作用，同时还会受到阻力D和侧倾力矩L的作用。同时，升力F，反扭矩Q，阻力D以及侧倾力矩L均与旋翼转速ω的平方成比例关系，具体的表述见式（2-3）：其中，CF为旋翼的拉力系数，CQ为旋翼的扭矩系数，CD为旋翼的阻力系数，CL为旋翼的侧倾力矩系数，ρ，R，A分别表示空气密度、旋翼桨叶半径以及桨盘的面积。可利用动量理论和叶素理论分别对相对气流和旋翼的几何特性进行分析，以准确求取旋翼的空气动力学系数。1）诱导速度的计算。诱导速度，可以反映出旋翼与其所处周围空气的相互作用，能够引起速度大小和方向变化。在旋翼改变空气的动量和动能的同时，气流对桨盘产生的反作用就形成了空气动力。以飞行器处于悬停状态为例，推导诱导速度。如图2.7所示，飞行器处于悬停状态，旋翼的滑流截面积为上面大下面小的形状，其中V0代表旋翼处的空气速度，v1代表旋翼桨盘处的诱导速度且与该处的空气速度V1相等，v2代表旋翼下方很远处的诱导速度且与该处的空气的速度V2相等。根据动量定理，针对旋翼滑流，有：其中，m′为单位时间内流过滑流任一截面的空气质量，F′为滑流所受到的合外力。根据质量守恒定律，可得：其中ρ为空气密度，V0，V1，V2为图2.7所示各区域的空气速度，S0，S1，S2为对应的区域截面积，C为正常数。因此，气流对旋翼桨盘产生的反作用力即为旋翼产生的升力F：根据能量守恒定律和旋翼滑流的动能变化率可确定旋翼消耗的功率：图2.7诱导速度根据式（2-6）（2-7）及（2-8）可得旋翼桨盘下方的诱导速度等于桨盘处的诱导速度的2倍，则：因此式（2-6）表达的旋翼产生的升力可重新表达为：其中，ρ为空气密度，A为旋翼桨的桨盘面积。根据式（2-10）可得飞行器处于悬停状态下的旋翼诱导速度为：同理，利用动量理论和能量定理，可推导出飞行器以速度v飞行时的诱导速度：2）叶素理论。叶素理论是假设旋翼气流无滑流收缩，将旋翼桨叶分成很多小的小段，即叶素，根据旋翼翼型可以对每个叶素上的空气动力进行计算，然后沿着径向进行积分求得旋翼的总的空气动力。叶素理论考虑了旋翼的几何特性和运动特性。首先，取桨叶上径向位置为r，宽度为Δr，弦长为c的叶素，对其进行受力分析，如图2.8所示。其中θs为桨叶叶素几何安装角，φs为来流角，αs为迎角，V0为飞行器的飞行速度。图2.8叶素受力分析根据图2.8可知，桨叶叶素的安装角θs、迎角αs与来流角φs之间有如下关系：其中，Up，Ut为来流在竖直和水平方向上的两个速度分量。和诱导速度v1的关系为：其中，v1为飞行器以某一速度V0飞行时旋翼的诱导速度，λ为流入比系数，μ为前进比系数。加载在叶素上的升力E和阻力D为：其中，ΔE称为基元升力，ΔD称为基元后向力，q为动压，Ct为叶素的基元升力系数，Cd为叶素的基元后向力系数。将基元后向力ΔD和基元升力ΔE在叶素坐标系上投影，便可得到如下基元阻力ΔFx和基元拉力ΔFy：因此根据式（2-16）可得ΔFx≈－ΔD，ΔFy≈ΔE。整个旋翼产生的升力F为：根据上述求取过程可得旋翼的反扭矩Q、阻力D以及侧倾力矩L，其表达式如下：将式（2-17）～（2-20）和式（2-3）对比，可分别得到拉力系数CF，扭矩系数CQ，阻力系数CD，侧倾力矩系数CL的表达式如下：其中，ϑ为旋翼的实度。2.5空气阻力1）桨叶挥舞。桨叶挥舞是由沿着桨叶气流入射速度分布不均匀引起的运动。前行和后行桨叶所受的升力有差异会导致升力不均匀，同时叶片非完全刚体则会引起桨尖扫过的平面发生倾斜，从而产生桨叶挥舞现象即桨叶的上下运动。如图2.9（a）所示，前行桨叶逆风前进，因此获得更大的相对速度。这时作用在螺旋桨的拉力增加，产生向上挥舞速度。如图2.9（c）所示，螺旋桨的向上挥舞速度会使攻角减小，进而减少拉力。图2.9桨叶挥舞2）多旋翼气动阻力模型。如图2.10（a）所示，多旋翼向右飞行，螺旋桨逆时针旋转，在A点处产生最大的相对速度。然而，螺旋桨最大的上偏位置出现在B点，其中A点比B点滞后π/2。这个道理如同正弦曲线运动规律一般[如图2.10（b）]。因此，如图2.10（c）所示，螺旋桨桨盘下方是前行螺旋桨（advancingblade）区，上方是后撤螺旋桨（retreatingblade）区。因为位置滞后速度，所以前面半区是螺旋桨上偏区，而后面半区是螺旋桨下偏区。螺旋桨的挥舞改变了桨盘的方向，从而进一步改变了拉力的方向。从图2.11可以看出，拉力不再与多旋翼的机体轴平行，而是在xb负方向上产生分量，即诱导阻力。该阻力是多旋翼阻力的主要组成部分，不容忽视。多旋翼气动阻力模型将以此为依据。多旋翼在机体轴xb，yb上的速度为，，则：阻力表示为：图2.10螺旋桨挥舞速度和偏移位置图2.11多旋翼飞行器升力阻力示意图其中fx，fy分别表示机体轴xb，yb上的阻力，而kdrαg∈R＋表示阻力系数，多旋翼的空气阻力模型如式（2-29），式（2-30）。2.6旋翼无人机模型的建立2.6.1受力在机体系和导航系之间的转换在建立四旋翼飞行器的动力学模型之前，先定义以下变量。飞行器的位置向量P＝（x，y，z）T∈R3是飞行器重心相对于惯性坐标系Oe（Xe，Ye，Ze）的坐标变量，飞行器的线速度向量v＝（v1，v2，v3）T∈R3是飞行器相对于惯性坐标Oe（Xe，Ye，Ze）的线速度，飞行器的角速度向量ω＝（ω1，ω2，ω3）T∈R3是飞行器相对于惯性坐标系Oe（Xe，Ye，Ze）的角速度，飞行器的姿态向量Θ＝（φ，θ，ψ）T∈R3是飞行器相对于惯性坐标系Oe（Xe，Ye，Ze）的姿态角变量，其中φ，θ，ψ分别表示飞行器的滚转角、俯仰角、偏航角，如图2.12所示。图2.12四旋翼飞行器姿态角表示其中，滚转角φ是机体坐标系Ob（Xb，Yb，Zb）相对惯性坐标系Oe（Xe，Ye，Ze）沿X轴的变化的角度，一般定义飞行器向右滚转时的角度为正。由机体坐标系Ob（Xb，Yb，Zb）到惯性坐标系Oe（Xe，Ye，Ze）沿X轴旋转的转换矩阵为：俯仰角θ是机体坐标系Ob（Xb，Yb，Zb）相对于惯性坐标系Oe（Xe，Ye，Ze）沿Y轴的变化的角度，一般定义飞行器头部上扬时为正。由机体坐标系Ob（Xb，Yb，Zb）到惯性坐标系Oe（Xe，Ye，Ze）沿Y轴旋转的转换矩阵为：偏航角ψ是机体坐标系Ob（Xb，Yb，Zb）相对于惯性坐标系Oe（Xe，Ye，Ze）沿Z轴的变化的角度，一般定义飞行器向右偏航时为正。由机体坐标系Ob（Xb，Yb，Zb）到惯性坐标系Oe（Xe，Ye，Ze）沿Z轴旋转的转换矩阵为：2.6.2欧拉角速度与机体角速度的转换机体坐标系Ob（Xb，Yb，Zb）到惯性坐标系Oe（Xe，Ye，Ze）的线速度转换矩阵R（Θ）∈R3×3是按照Zg，Yg，Xg的顺序将机体坐标系Ob（Xb，Yb，Zb）旋转得到的。其数学表达式为：角速度转换矩阵RT（Θ）的一阶导数满足以下性质：式（2-37）中矩阵S（ε）∈R3×3为一个反对称矩阵，其具体形式为：2.6.3旋翼无人机的动力学模型四旋翼飞行器控制器设计的基础是飞行器的动力学模型，前文我们已经阐述过可以把四旋翼飞行器看作刚体来进行研究，且飞行器的重心与机体坐标系的原点重合，忽略飞行器运动时由螺旋桨和电机产生的振动，以及飞行器受地面效应的影响。在上述假设下，四旋翼飞行器的运动可分为：重心沿惯性坐标系X，Y，Z三个坐标轴的线运动，以及绕惯性坐标系三个坐标轴的转动，即滚转、俯仰、偏航。对于四旋翼飞行器，大多数研究人员都是依据角动量守恒定律和牛顿第二定律进行飞行器的动力学建模。根据牛顿第二定律可知，建立四旋翼飞行器的动力学方程如式（2-39）所示：其中，m为整个飞行器的总质量，包括供电设备（铿电池）和机架、螺旋桨电机等的质量，因此飞行器的质量不会发生改变；F为飞行器运动过程中所受的合外力。飞行器所受的外力包括重力、阻力、升力。其中ˉG＝（0，0，g）T∈R3为重力向量（g＝9.8m/s2，为重力加速度）。在机体坐标系中，单个螺旋桨所产生的升力为：式（2-41）是在惯性坐标下建立的物理方程式，因此要把两个物理方程式统一在同一坐标系中，利用线速度转换矩阵R（Θ）∈R3×3，统一到惯性坐标系中，如式（2-42）：飞行器在运动的过程中受到的阻力为：化简整理得：式（2-45）为四旋翼飞行器的线性位移的运动数学模型。根据角动量守恒定律，惯性坐标系下飞行器的转动方程式如式（2-46）所示。飞行器的角速度向量和飞行器的姿态向量有如下关系：则式（2-47）化简整理可得：其中，MB为机体坐标系下的合外力矩。四旋翼飞行器在运动过程中，螺旋桨的旋转除了给飞行器提供一个向上的升力外，每个螺旋桨还产生一个扭矩。设旋翼到重心的距离大小为l，且四个旋翼与重心的距离都相等，则每个旋翼所产生的力矩大小为：由四旋翼飞行器的飞行原理可知，俯仰力矩大小为：滚转力矩大小为：偏航力矩大小为：其中，λ为比例系数，其大小由旋翼半径、空气密度、叶片数等决定。在四旋翼飞行器运动过程中，当飞行器的飞行姿态发生变化时，对于高速旋转的螺旋桨和直流无刷电机的转子，由于陀螺效应会产生一个附加的无法避免的陀螺力矩，其数学表达为：其中Hi为总动量矩，定义Jr为绕z轴转动的惯量，动量矩的数学表达式为：将式（2-54）代入式（2-53）化简整理得：则四旋翼飞行器的总合外力矩为：将式（2-49）（2-50）（2-51）（2-56）代入式（2-48）化简整理得：由式（2-47）得到机体角速度与姿态的方程如下：综上所述，四旋翼飞行器的运动学方程为：第3章旋翼无人机的运动信息估计3.1旋翼无人机系统中传感器简介3.1.1九轴惯性测量单元1）惯性测量单元。惯性测量单元（inertialmeasurementunit，简称IMU）是测量物体三轴姿态角（或角速率）以及加速度的装置。IMU大多用在需要进行运动控制的设备上，如汽车和机器人；也被用在需要用姿态进行精密位移推算的设备，如潜艇、飞机、导弹（见图3.1）和航天器的惯性导航设备等。图3.1导弹上的惯性测量元件MEMS三轴加速度计2）技术背景。传统的MEMS惯性传感设备的成本非常低，体积极小，质量很轻，输出稳定且不易受到干扰，对外部环境条件的适应性极强，而且现在几乎随处都能买到。应用于旋翼无人机控制的九轴惯性测量传感器包括三轴陀螺仪、三轴加速传感器、三轴磁感应传感器。三轴陀螺仪最大的作用就是测量角速度，以判别物体的运动状态，所以也称为运动传感器。加速度计是惯性导航和惯性制导系统的基本测量元件之一，其本质上是一个振荡系统，可以用来测量载体的运动加速度。磁力仪中的磁力计采用垂直的结构，每个磁力计轴上的传感器检测在该方向上的地磁场强度。3）惯性测量装置IMU的工作原理。三轴加速度计是一个微机电系统（MEMS）元件，如图3.2所示。MEMS三轴加速度计是采用压阻式、压电式和电容式工作原理，产生的加速度（压力或者位移）分别正比于电阻、电压和电容的变化。这些变化可以进行放大处理之后通过滤波电路进行采集。该传感器的缺点是受振动影响较大。由于大多数MEMS传感器是以半导体材料作为基础加工的，而半导体对温度很敏感，再加上安装、电路设计等影响因素，零点漂移和温度漂移不可避免，但可以通过滤波算法得到更准确的数值。基于MEMS技术的三轴加速传感器配合陀螺仪或电子罗盘等元件可创建方位推算系统，对定位系统实现互补性应用。图3.2MEMS加速度测量原理陀螺仪的英文名为gyroscope，是一种用来传感和维持方向的装置设备。陀螺仪是由一个位于轴心且可旋转的轮子构成。陀螺仪一旦开始旋转，由于轮子的角动量，陀螺仪有抗拒方向改变的趋向。陀螺仪可分为单轴陀螺仪和三轴陀螺仪两种。单轴陀螺仪只能测量一个方向，系统测试三维空间就需要三个。而三轴陀螺仪就可以同时测量6个方向。三轴陀螺仪多用于航海、航天等导航、定位系统，它能够精确地确定运动物体的方位，其物理模型如图3.3（a）所示。MEMS陀螺原理如图3.3（c）所示，两个质量块运动速度方向相反，而大小相同。它们产生的科里奥利力相反，从而压迫两块对应的电容板移动，产生电容差分变化。电容的变化正比于旋转角速度。因为加速度变化只能导致两个质量块朝相同方向移动，不会带来电容的差分变化。三轴陀螺仪在实际生产和安装过程中，总会出现一些偏差。因此，实际应用时需要建立标定前和标定后的三轴角速度的关系。图3.3（b）所示为三个互相垂直的磁力计，每个轴向上的传感器检测在该方向上的地磁场强度。这种具有晶体结构的合金材料对外界的磁场很敏感，磁场的强弱变化会导致AMR（anisotropicmagneto-resistance，各向异性磁致电阻，见图3.4）自身电阻值发生变化。另外还可以采用洛伦兹力原理，电流流过磁场产生力，从而驱动电容等变化。图3.3陀螺仪及MEMS原理图3.4磁致电阻当有外界磁场Hα时，AMR上主磁域方向就会发生变化而不再是初始的方向了，那么磁场方向和电流的夹角θ也会发生变化，如图3.5所示。对于AMR材料来说，θ角的变化会引起AMR自身阻值的变化，并且呈线性关系，如图3.5所示。图3.5磁致电阻作用原理三轴电子罗盘在实际生产和安装过程中，总会出现一些偏差，不仅如此，周边的原件（飞控芯片、供电电路、电机）也会对其产生影响。要想减小误差也需要相应的模型分析和校正方法。3.1.2GPS定位系统（见图3.6）是指利用卫星，在全球范围内实时进行定位、导航的系统，简称GPS（globalpositioningsystem）。例如美国的GPS、中国的北斗星、俄罗斯的GLONASS、欧洲的伽利略都属于卫星定位系统。图3.6定位系统虽然惯性导航系统能够连续工作并有效地提供姿态信息、位置信息和速度信息，但由于惯性传感器误差的积累，惯性导航系统的工作精度会随时间下降。GPS可以提供长时间的误差为米级的高精度位置输出且用户设备成本较低，但由于GPS信号会被遮挡或干扰，因此不能仅依赖GPS提供连续导航参数（见图3.7）。鉴于INS（inertialnavigationsystem，惯性导航系统）和GPS系统的优缺点互补，飞控系统中经常将二者组合在一起，综合两系统的优势，提供有效、长时、高精度、完整的导航参数。图3.7无人机搭载的GPS接收机3.1.3光流点在像平面的运动称为光流。光流是测速算法，并不能直接定位。简单理解，光流就是通过检测图像中光点和暗点的移动，来判断图像中像素点相对于飞行器的移动速度。如果地面是静止的，自然就可以得到飞行器相对于地面的移动速度。光流定位其实是利用光流测速（见图3.8）再积分定位。图3.8光流测速3.1.4气压计多旋翼多采用压电式气压计（见图3.9），气压计也是一种压力传感器，其原理是许多晶体（如石英、碧玺、酒石酸钾钠等）按一定轴向受压时，会在表面上产生电荷（压电现象），电荷的量与所受压力成正比。新一代的气压计既可以测量气压和高度，又可以测量温度。大气压随高度的增加而减小，气压计正是通过测量大气压来估计高度。由于大气压分布不是均匀的而且气压计对气流的影响很敏感（有风情况测量不准），因此气压计只能得到飞行高度的近似值。图3.9气压计外观图3.2系统可观性分析图3.10系统状态图可观测性表示输出可以完全反映系统状态的特性，因此不仅要考虑系统的状态方程还要考虑输出方程。以考虑连续时间线性时变系统∑为例，其状态方程和输出方程分别为：其中t，t0∈J，x为n维状态，u为p维输入，y为q维输出，t0为初始时刻，A（t）为n×n维的系统矩阵，B（t）为n×p维的输入矩阵，C（t）为q×n维的输出矩阵，D（t）为q×p维的传输矩阵，A（t）的元在J上为绝对可积，B（t）的元在J上为平方可积。对系统的运动进行分析可知，系统∑状态解的表达式为：其中φ（t，τ）为系统的状态转移矩阵，δ（t）为单位冲激函数。在许多实际问题中，控制系统的状态变量不是由直接测量得到的，而是通过某种观测方法得到的，某种观测系统所得到的结果对系统状态真实性的反映就是系统的可观测性。可观性反映系统外部可直接或间接测量的输出y（t）和输入u（t）来确定或识别系统状态的能力。如果系统的任何内部运动状态变化都可由系统的外部输出和输入唯一地确定，那么称系统是能观的，或者更确切地说，是状态能观的。否则，就称系统为状态不完全能观的。下面通过例子来说明可观性的意义。例3-1：给出两个传感器GPS和加速度计，那么哪一个能够稳定地估计出多旋翼速度？为了简化起见，我们用GPS观测位置，一般用如下模型：其中x，v∈R分别表示位置状态和速度状态，ε∈R表示已知统计特性的噪声。为了简化起见，用加速度计估计速度，一般用如下模型：其中v，α∈R分别表示速度大小和加速度大小，ε∈R表示已知统计特性的噪声。3.3姿态估计3.3.1互补滤波1）互补滤波基本概念。对无人机上的惯性测量单元来说，加速度计对四轴的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度不受加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。所以这两个传感器正好可以相互弥补缺点。短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了。而滤波指的是加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号。例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺仪响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好，通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加速度计的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率。2）用于姿态解算的互补滤波原理及应用。图3.11准确地阐述了互补滤波的过程。正常情况下用陀螺仪的数据就可以进行姿态的更新，但是由于陀螺仪的积分误差，这里用acc（加速度）和mag（地磁信息）去校正，求出他们的误差用D去弥补。注意看看PID的公式和作用，PID是作用于误差（实际与期望之间的差值），最终反复调节，让实际值等于期望值。图3.11互补滤波流程图3.3.2梯度下降法梯度下降法（gradientdescent）是一个一阶最优化算法，通常也称为最速下降法。要使用梯度下降法找到一个函数的局部极小值，必须沿函数当前点对应梯度（或者是近似梯度）的反方向按规定步长进行迭代搜索。如果相反地向梯度正方向进行迭代搜索，则会接近函数的局部极大值点，这个过程则被称为梯度上升法。在互补滤波法中，我们使用加速度计来补偿陀螺仪，将两者的数据融合解算出四元数下的姿态。在梯度下降法中，依然是通过加速度计的数据来与陀螺仪的数据进行融合。而不同点是：互补滤波法中，对载体坐标系b下的加速度做叉积以求出误差，补偿给陀螺仪以校正误差；梯度下降法中，通过加速度计的数据求出一组四元数q，然后通过四元数的微分方程，并使用陀螺仪得到的数据求解出另外一组四元数qῶ。因为在高速运动状态下，陀螺仪数据更可靠；而在低速运动状态下，加速度计数据更可靠，所以两组四元数分别乘以权重再相加，就得到了期望的输出结果。使用梯度下降法求解姿态的算法介绍：假设一个误差函数ef（x），我们希望其满足：ef（x）＝0，即误差为0。接着，我们要求解这个方程，得到x的值。此时x值对应最优解。使用梯度下降法求解，首先我们就需要先求出它的导数将x换成四元数Q＝（q0，q1，q2，q3）。函数也相应变化，其导数变为如下形式：因为我们求解的姿态是三维姿态，而上式的因变量只对应了一维的情况。所以将因变量推广到xyz轴下，变成一个多元向量函数：求它的导数：当这个误差函数为0（即最小值）时，我们认为旋转矩阵没有误差，姿态是精确的。那么，接下来的任务就是使用前面介绍的梯度下降法相关公式来求解了。用前面阐述的方法求这个多元向量函数的导数（即它的雅可比矩阵）：代入公式求出梯度：通过离散叠加替代积分可以求解出一组四元数xn，将其乘以权重与陀螺仪数据通过四元数微分方程求解得到的另一组四元数qῶ。最终的公式为：其中qg为最终输出的四元数，α为权重，q为加速度计的数据通过梯度下降法得到的四元数，qῶ为陀螺仪的数据通过四元数微分方程求解得到的四元数。3.3.3卡尔曼滤波1）卡尔曼滤波算法的基本思想。卡尔曼滤波（Kalmanfiltering），一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。因为观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作滤波过程。算法的核心思想是，根据当前的仪器测量值和上一刻的预测量和误差，计算得到当前的最优量，再预测下一刻的量。里面比较突出的是观点是把误差纳入计算，而且分为预测误差和测量误差两种，统称为噪声。还有一个非常大的特点是误差独立存在，始终不受测量数据的影响。2）卡尔曼滤波算法的原理及推导公式。我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：再加上系统的测量值：上两式子中，X（k）是k时刻的系统状态；U（k）是k时刻对系统的控制量；A和B是系统参数，对于多模型系统，它们为矩阵；Z（k）是k时刻的测量值；H是测量系统的参数，对于多测量系统，H为矩阵；W（k）和V（k）分别表示过程和测量的噪声。它们被假设成高斯白噪声（whiteGaussiannoise），它们的协方差分别是Q，R（这里假设它们不随系统状态变化而变化）。当满足上面的条件时（线性随机微分系统中产生的噪声都是高斯白噪声），卡尔曼滤波器是最优的信息处理器。下面用它们结合协方差来估算系统的最优化输出。首先，我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：式（3-1）中，X（k∣k－1）是利用上一状态预测的结果，X（k－1∣k－1）是上一状态最优的结果，U（k）为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，系统结果已经更新了，可是对应于X（k∣k－1）的协方差还没更新。用P表示协方差：式（3-2）中，P（k∣k－1）是X（k∣k－1）对应的协方差，P（k－1∣k－1）是X（k－1∣k－1）对应的协方差，A′表示A的转置矩阵，Q是系统过程的协方差。式（3-1）（3-2）就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。有了现在状态的预测结果，再收集现在状态的测量值。结合预测值和测量值，可以得到现在状态（k）的最优化估算值X（k∣k）：其中gK为卡尔曼增益（Kalmangain）：到现在为止，已经得到了（k）状态下最优的估算值X（k∣k）。但是为了让卡尔曼滤波器不断地运行下去直到系统过程结束，还要更新（k）状态下X（k∣k）的协方差：其中I为1的矩阵，对于单模型单测量，I＝1。当系统进入（k＋1）状态时，P（k∣k）就是式（3-2）的P（k－1∣k－1）。这样，算法就可以自回归地运算下去。这就是卡尔曼滤波器原理的基本描述。式（3-1）（3-2）（3-3）（3-4）和式（3-5）就是他的5个基本公式。根据这5个公式，可以很容易地实现计算机的程序。3.4位置估计3.4.1加速度计积分加速度是一个对象速度的变化速率，速度是一个对象位置的变化速率。换句话说，速度是位置的导数，加速度是速度的导数，因此有如下公式：积分和导数相反。如果一个物体的加速度已知，那么我们能够利用二重积分获得物体的位置。假设初始条件为0，那么有如下公式：一个理解这个公式的方法是将积分定义成曲线下面包围的区域，积分运算结果是极小区域的总和，区域的宽度趋近于0。换句话说，积分的和表示了一个物理变量的大小（速度）。利用曲线下方区域的概念，我们能得出一个结论：对一个信号采样，得到该信号大小的瞬时值，这样就能够在两次采样之间得到一个小的区域。为了获得连贯的值，采样时间必须相同。采样时间代表这块区域的宽，同时采样得到的值代表区域的高。设加速度函数为x（t），则速度函数y（t）与它的关系如式（3-6）所示，x（t）拉氏变换函数为x（s），y（t）拉氏变换为y（s），则将式（3-6）进行拉氏变换后如式（3-7）所示：所以加速度积分的模型如图（3.12）所示，图中x（s）为加速度的S域函数，y（s）为积分后的速度S域函数。由1/s的Z变换为1/（1－1/z）可以将式（3-7）转换到Z域，如下式：由上式可以进一步得到加速度和积分后的速度的差分方程关系：其中y（k）是速度的第k个采样点，y（k－1）是速度的第k－1个采样点，x（k）是加速度的第k个采样点。图3.12加速度积分积分会对误差积累，随着时间的推移积分出的误差就会越来越明显。因此实际测得的加速度零点可能偏上或者偏下，这样就会导致积分出的速度整体以一定的斜率向上或者向下漂移，需要对计算出的速度进行误差补偿。3.4.2组合导航（GPS与IMU）组合导航技术结合GPS、惯性测量元件、地磁指南针和气压计各自的优缺点，使用电子信号处理领域的很多技术，融合多种传感器的测量值，获得较为准确的飞行器15个状态量的测量。惯性测量元件的测量容易发散，这个发散可以通过GPS来抑制。GPS可以获得三维位置也可以获得三维速度，惯性测量元件可以获得三维加速度，加速度的积分也是速度。在通过地磁指南针获得航向的基础上，两种速度的观测就可以融合起来，通过GPS的测量值来发现并抑制惯性测量元件的发散。惯性测量元件的发散被抑制住之后，它也可以更准确地测量三维角度和三维加速度。因此GPS和惯性测量元件在这些情况中互相取长补短。除此之外，气压计和GPS互相提高了高度测量的精度，地磁指南针、GPS和惯性测量元件一同提高了航向测量的精度，它们都是利用了相同的融合、互补的思想。组合导航技术中传感器互补的原理直接源于1948年诞生的信息论。克劳德·香农总结归纳出的信息论提出了信息的概念以及如何从数学上度量信息，信息论可以说是现代人类文明的基石之一。解释清楚信息的本质之后，人们才能够用数学表示一个朴素而又深刻的原理：信息可以用来估计状态，信息越多，状态量被估计得越准。GPS是一种基于卫星的无线电导航系统。GPS可为各类用户连续地提供动态的三维位置、三维速度和时间信息，实现全球、全天候的连续实时导航、定位和授时。加之GPS设备成本低、体积小等优点，GPS技术在导航和测量各领域得到了广泛的应用。惯性测量单元（inertialmeasurementunit，IMU）具有自主性强、隐蔽性好和信息完备的特点。而系统定位误差随时间持续积累，即使是高精度的INS系统长期工作误差也会越来越大，必须进行修正。GPS技术和惯性技术各有优势，但在误差传播性能上正好是互补的。INS的误差会随时间积累而越来越大；而GPS的误差不随时间积累。INS短期精度好，尤其在高动态环境下精度仍然较高；而GPS在长时间工作时表现出色，但不适用于高动态环境。高精度的GPS信息，作为外部量测输入，在运动过程中频繁修正INS，以控制其误差随时间的积累；短时间高精度的INS定位结果，可以很好地解决GPS动态环境中的信号失锁和周跳问题，而且INS还可以辅助GPS接收机增强其抗干扰能力，提高捕获和跟踪卫星信号的能力。因此把这两项技术结合起来可以得到高精度的导航系统。1）组合导航系统的卡尔曼滤波器。MEMS-IMU/GPS组合导航系统的基本构成为：MEMS-IMU、GPS接收机和导航计算机。导航系采用东北天地理坐标系，各传感器信息通过最优卡尔曼滤波器进行融合，系统的工作原理如图3.13所示。图3.13组合导航系统的工作原理考虑陀螺仪随机误差为一阶马氏过程和白噪声，加速度计随机误差为白噪声，系统的误差状态变量定义为：其中，φE，φN，φU为东向、北向和天向的数学平台误差角；δvE，δvN，δvU为东向、北向和天向速度误差；δφ，δλ，δh为纬度、经度和高度误差；εrx，εry，εrz为陀螺仪一阶马氏过程误差。列写状态方程如下：图3.14所示的系统中，采用常规松组合的方式进行MEMS-IMU和GPS的速度、位置组合。由于MEMS-IMU的精度非常低，航向角发散速度相当快，GPS的航迹角虽然不是真航向，但在很大程度上也能反映出载体的航向变化，故将GPS航迹角与MEMS-IMU的航向进行组合，从而维持系统航向的有效输出。为了增加系统姿态信息的冗余度，特别是在GPS信号失效时保持系统的姿态稳定，利用MEMS-IMU中加速度计的比力信息间隔性地估计姿态角（滚转角和俯仰角），并与MEMS-IMU输出的姿态角进行组合。加速度计估计的姿态角可表示为：姿态观测方程可表示为：其中，Hα为姿态观测矩阵；Vα为观测噪声；Zα为观测量，由MEMS-IMU解算的姿态（y1，θ1）和加速度计估计的姿态（yD，θD）之差构成，即：由于系统状态方程中的误差角为数学平台误差角，它描述了数学平台和地理坐标系之间的关系，而姿态观测方程中的姿态误差角描述了载体和地理坐标系之间的关系，因此，不能将姿态观测矩阵简单定义为单位矩阵。通过推导可得姿态误差角与数学平台误差角之间存在如下的转换关系：综上所述，MEMS-IMU/GPS组合导航系统的观测方程可表示为：其中，H为观测矩阵，V为观测噪声，Z为观测量。2）信息融合策略。MEMS-IMU/GPS组合导航系统的信息融合策略表述为：①系统在工作过程中，MEMS-IMU每隔ΔT＝0.01s采样一次，GPS每隔TG＝0.1s输出载体速度、位置和航迹角信息，对各测量值剔除野值和粗大误差。②MEMS-IMU通过级联解算获得载体的姿态、速度和位置信息，加速度计每隔TA＝0.1s计算滚转角和俯仰角。③当系统导航时间t为TG（或TA）的整数倍时，采用最优卡尔曼滤波进行姿态、速度和位置组合。④当观测信息有效时，利用卡尔曼滤波器的估计值更新系统的姿态、速度和位置，并对系统进行反馈校正；当观测信息无效时，卡尔曼滤波器只进行时间更新，并直接输出MEMS-IMU的导航结果。3.4.3光流（室内）定位在无人机上光流定位通常是借助于无人机底部的一个摄像头采集图像数据，然后采用光流算法计算两帧图像的位移，进而实现对无人机的定位，这种定位手段配合GPS可以在室外实现对无人机的精准控制，并且在室内没有GPS信号的时候，也可以实现对无人机的高精度的定位，实现更加平稳的控制。在光流理论中，前提是下面两个假设成立：①摄像头采集到的两帧图像之间的像素灰度不变。②相邻的两帧图像的像素具有相对运动。根据第一个假设，如果两帧图像的灰度值不变，那么以下关系式成立：其中I（x，y，t）表示在时间dt后移动到第二帧图像（x＋dx，y＋dy）的位置，采用泰勒级数对两边进行展开，消去相同的项，就可以得到如下方程：其中：以上就是光流方程，其中fx和fy表示图像的梯度，ft表示时间梯度。但是上述方法是无法得到（u，v）T，因为一个等式无法求解两个未知数。为了解决这个问题，我们可以采用经典的Lucas-Kanade方法来进行求解。在Lucas-Kanade方法中，我们需要用到第二个假设，即在目标点的邻域内所有的点都具有相似的运动，这就是Lucas-Kanade方法的核心，基于该假设，其利用一个3×3邻域中的9个点具有相同运动得到9个光流方程，然后采用最小二乘法进行拟合求解，最终得到（u，v）T为：以上就是光流法计算像素点的移动速度的方法，在使用的时候，我们只需要对图像中的一些点进行跟踪，采用上面的方法就可以计算得到光流向量，根据得到的光流向量，就可以进一步优化无人机的姿态控制，实现更加准确的控制。第4章旋翼无人机视觉定位方法及系统4.1视觉定位方法简介自主导航是移动机器人领域中的关键技术之一。近年来，随着机器人系统运算能力和图像处理等相关技术的发展，人们开始越来越多地研究不依赖于GPS系统的自主导航技术，这是因为GPS系统存在精度不够高、更新速度慢以及数据容易跳变等缺陷；而且在室内环境中，往往无法获得GPS信号；天气条件、云层太厚也会影响GPS信号的接收。基于视觉的飞行器位姿测量是当前旋翼飞行器导航的研究热点，常用的飞行器视觉位姿测量方法可以分为外部视觉方法和内部视觉方法两种。内部视觉方法即通过机载的图像传感器来获得位姿信息，又叫机载视觉定位，它不依赖于外部信号强度，具有抗干扰能力强、适应性好的特点。机载视觉系统按照视觉传感器的种类可以分为双目及多目视觉系统、单目视觉系统两种。双目及多目视觉系统，以Bumblebee2为例，该物理系统主要由两个单目摄像机组成，其相互之间标定工艺复杂，价格昂贵，但是适用范围广，精度十分高。和双目及多目视觉系统相比，单目视觉系统不需要摄像机之间相对标定，且不受摄像机安装误差的影响，价格便宜，传感器质量小，运算量小，但是需要目标点之间有几何约束关系，且需要至少4个目标点成功匹配才能获取无人机精确的位姿，因此应用场景有限制。外部视觉以Kinect为例，在获得彩色图像的同时，通过对红外数据的处理获得图像中每个点的深度数据，从而精确获取四旋翼飞行器的位置和姿态信息，但是它的缺点也是显然的，当飞行器到室外或是一些无法安置摄像头的地方，就无法实现视觉定位。4.2相机标定相机的精确标定是无人机位姿精确求解的必要条件。对于单目视觉来说，相机标定就是获取相机内参以及畸变参数；对于双目视觉来说，既要获取内参也要获取外参。当然，相机标定在OPENCV或MATLAB中都有现成的函数，只要知道如何调用就可以了。但是之所以单独列出一节，是为了让读者可以透过函数看到外表下的内涵。在了解标定过程之前，我们需要了解到图像坐标系是如何与世界坐标系联系在一起的，并且坐标系之间的关系不仅仅关乎标定过程，还是视觉定位的原理及其基础。这个过程需要一步一步地分析，我们试图通过摄像机坐标系来搭建一个桥梁。在此之前，为了方便读者理解，先列出一些名词的解释。世界坐标系（xw，yw，zw）：是目标物体位置的参考系。除了无穷远，世界坐标可以根据运算方便与否自由放置。相机坐标系（xc，yc，zc）：是摄像机站在自己角度上衡量的物体的坐标系。摄像机坐标系的原点在摄像机的光心上，z轴与摄像机光轴平行。图像坐标系（x，y）/（u，v）：是以摄像机拍摄的二维照片为基准建立的坐标系。读者也许已经发现了，如图4.1所示相机拍摄出来的图片都是以像素的形式储存的，由于存储顺序的原因，拍摄出来的坐标系的原点并不在图像的中心，这并不方便我们后续的建模，所以我们需要将坐标原点在左上角的（u，v）形式的图像坐标系转换为坐标原点在中心的（x，y）形式的图像坐标系。图4.1坐标系之间的关系显然，（x，y）与（u，v）存在如下转换关系：其中，dx代表x轴方向一个像素的宽度，dy代表y轴方向上一个像素的宽度。dx，dy为摄像机的内参数。（u0，v0）是图像平面的主点，位于摄像机光轴与成像平面的焦点O′处，也是摄像机的内参数。根据式（4-1）和（4-2），我们可以写出式（4-3）这个矩阵形式：接着我们试图走出图像坐标系，将图像坐标系与相机坐标系连接起来。在这里，我们先假设主点就是成像仪的中心（实际上芯片的中心通常不在光轴上，这是因为我们无法对成像仪进行微米级别的精度安装，在后面的内容中我们将考虑这个误差），然后我们可以利用小孔成像的经典模型进行建模，如图4.2所示。图4.2小孔成像模型这里为了将模型简化，将成像平面放在了小孔前面，形成了如图4.3的模型。在数学上这两个模型是等价的，并且后者更方便我们建模。图4.3透视投影模型不妨设在相机坐标系上的一点为（xc，yc，zc），图像平面上所对应的一点为（x，y），我们可以很容易地写出如下公式：显然，我们可以根据式（4-4）和（4-5）写出式（4-6）这个矩阵形式：至此也就完成了图像坐标系与相机坐标系的联系。接下来，只剩下最后一步就可以将图像坐标系与世界坐标系联系起来。如图4.1所示，我们可以看到相机坐标系与世界坐标系只差一个旋转和平移的过程。基于刚体变换的模型，我们写出式（4-7）：基于上面讲的内容，我们可以得出如图4.4的结果。图像坐标系就是这样与世界坐标系联系在一起的。图4.4图像坐标系与世界坐标系之前的内容我们提到过主点并不是成像仪的中心，尽管可能误差并不大，但是我们仍然需要科学严谨地面对这些问题。其实只需在式（4-4）和（4-5）上作出一些修改，如式（4-8）和（4-9）所示。其中cx和cy代表主点与成像仪中心的可能的偏移，写成矩阵形式如下：尽管这样的偏移是客观存在的，但是由于这只是建模的过程，这样的误差是可以容忍的。综合式（4-3）（4-6）和（4-7），我们可以写出式（4-11）。因为标定物是平面，所以我们可以把世界坐标系构造在zw＝0的平面上。那么式（4-11）可以改写成式（4-12）。得出上述式子我们就可以开始标定的过程了。从式（4-14），我们可以得出如下结论：我们只需要已知四个点在图像坐标系和世界坐标系的位置，就可以求出单应性矩阵H。为了方便后续推导作以下运算：定义：那么，我们有其中结合式（4-16）和式（4-17），我们得出：根据式（4-23），我们发现只需要三幅图，就可以求出B矩阵，再使用Cholesky分解，就可以轻松地得到摄像机的内参矩阵A。内参矩阵求出来了，外参矩阵自然也求出来了，这是由于A是可逆矩阵。尽管只需要三幅图与四个点我们就可以求出内参与外参矩阵，但是为了让求出的结果更鲁棒，我们更倾向于使用棋盘格的标定板，在这上面将会有许多可以检测到的特征点，并且我们会多次移动相机或标定板，每移动一次获取一幅新的图像。最后通过最小化来获取内参矩阵和外参矩阵，通过这样的方法我们可以使得求出的结果不再只是拘泥于物理模型，而是具有了真正的实际物理意义。4.3Kinect外部视觉定位Kinect是微软公司开发的3D体感摄像机，开始用于体感游戏，目前广泛用于机器人视觉系统。Kinect外观如图4.5所示。图4.5Kinect外观RGB彩色摄像头用于彩色图像的摄取，深度图像信息采用的是主动获取方式，红外发射器发射红外线，红外传感器对反射回来的红外线进行接收，通过对反射信息的处理，就能获得每个像素的深度值。Kinect是目前最便宜的三维深度摄像头。为了精确获取四旋翼飞行器的位置信息，设计了一套基于Kinect的旋翼飞行机器人视觉定位系统。Kinect是一款RGB-D传感器，即在获得彩色图像的同时，通过对红外数据的处理获得图像中每个点的深度数据，配备了1080P的高清摄像头，采用专用的USB3.0端口传输数据，数据帧率最高可达50帧/秒。基于Kinect的外部视觉定位系统结构示意图如图4.6所示，在3m×3m×3m的室内空间中，搭建了实验平台，其中Kinect位于正中心的上方。Kinect利用光编码技术计算深度信息，通过USB3.0与主机相连，四旋翼飞行器通过蓝牙与主机通信，接收主机的控制指令和主机所计算的坐标数据。地面主机上运行使用跟踪定位软件，软件可以通过Camshift等算法跟踪安装在飞行器表面的彩色标记以及向飞行器发送彩色目标所在的位置信息，利用该视觉定位系统可以控制飞行器飞行到目标附近，并将目标所在的三维坐标信息输出给机械臂，操纵机械臂完成目标抓取作业。Kinect所发出的红外散斑具有高度的随机性，而且会随着距离的不同变换图案。也就是说，空间中任意两处的散斑图案都是不同的。因此，根据物体上面的散斑图案，就可以计算这个物体距离Kinect平面的距离。第二代Kinect配置了1080P的2D彩色摄像头，水平视野达到70°，垂直视野达到60°。Kinect坐标系的坐标原点位于彩色相机镜头的中心（见图4.7）。设彩色图像处理计算得到的物体重心在Kinect彩色图像中的坐标为（x，y），图像坐标的原点在图像的左上角，必须首先将图像坐标转换成以图像中心点为原点的坐标值（x1，y1），计算公式为：图4.6基于Kinect的外部视觉定位系统结构示意图其中，H和W分别是图像分辨率的高和宽，H＝920dpi，W＝1080dpi。由于Kinect彩色摄像头的视野范围（FOV）为垂直60°和水平70°，由此可以求出FOV与像素的关系约是0.065°/pixel。进而可以求得图4.7中的水平转角θh和竖直转角θv如下。图4.7Kinect坐标系的侧视图与俯视图然后根据Kinect深度信息zw，可以求得在Kinect坐标系中的目标坐标（xw，yw，zw），下标w表示世界坐标。由此就完成了基于Kinect视觉系统的图像坐标系到世界坐标系的映射。再通过Camshift或其他检测追踪算法实时捕捉到无人机的图像，就可以基于此图像实时获取无人机在世界坐标系下的位置。该方法的好处是简单易懂，花费不高，精度准确，但局限性也是一目了然的，因为有许多地方并不适合安装外部视觉的摄像头。4.4单目视觉系统顾名思义，单目视觉系统指的是仅仅通过一个摄像头来实现无人机位姿的测量。通过n个世界点和与之对应的图像坐标点以及摄像机的内部参数来计算摄像机方位的问题称为n点透视问题，简称为PnP问题。在这里，我们以EPnP算法为例讲解飞行器的位姿估计过程。值得注意的是，内部参数已经在之前的相机标定过程中得以求解。接下来我将首先讲解一下EPnP算法的原理，尽管这个算法与其他算法一样都已经有现成的函数，但是我们依旧希望能从前人的智慧中窥见一些对未来的展望，这不仅仅是有助于思维的扩展或是对前人贡献的致敬，更是为日后方便读者基于这些理论知识进行创新。在开始介绍算法之前，出于方便，我们先给出一些符号的定义：：世界坐标系的参考点坐标；：对应于世界坐标系下参考点在相机坐标系下的坐标；cj：控制点坐标。假设我们已知n个参考点的三维坐标以及它们各自对应的图像坐标点。对于一般情况，即n个参考点不在一个平面，我们可以给定4个不共面的控制点的不同的线性组合来分别表示这n个参考点；对于n个参考点在同一个平面，我们只需要3个控制点就可以表示这n个参考点，但是这只是上述情况的特殊情况，其实是包含在原问题里的。我们用4个给定的控制点来表示参考点坐标，如式（4-30）所示。由于世界参考点与控制点的坐标都是已知的，所以αij也容易求得。同理，我们可以写出相机坐标系下的关系式（4-31）：值得注意的是式（4-30）中的αij与式（4-31）中的是同一个，这个原因也是显然的，这无非是用不同的坐标系来描述同一件事情，因此它们的线性组合系数是不变的。结合式（4-11）和内参矩阵A易推得式（4-32）：在这里，我们忽略γ对后续结果的影响，将式（4-32）改写成式（4-33）：将式（4-34）和式（4-35）写成矩阵形式并简写如下：其中M是一个2n×12的矩阵，且容易发现矩阵M中的系数全是已知量，看上去我们应该直接求解这个线性方程组，这样就可以获取4个控制点在相机坐标系的坐标，然而这样的计算量无疑是很大的，在这里我们采用了巧妙的方法求解该问题。众所周知，M