机器人智能感知基础 课件 第8、9章 智能感知技术在机器人系统中的应用、基于多传感器信息融合的自动驾驶汽车位姿估计

1第八章智能感知技术在机器人系统中的应用目录CONTENT123移动机器人智能感知飞行机器人智能感知空间机器人智能感知45水下机器人智能感知仿生机器人智能感知移动机器人智能感知8.1各种传感器相当于移动机器人的手、眼、耳和鼻等器官，有助于识别自身的运动状态和环境状况。在这些信息的帮助下，控制器可以发出相应的指令，使机器人完成所需的动作。视觉传感器采集的图像由处理器进行处理，提取出对特定任务有用的信息。视觉传感器主要包括各种摄像机，比如RGB摄像机、多光谱摄像机和深度摄像机。摄像机中的光敏元件通常是CCD或者CMOS，都是利用光电效应原理，将光信息转换成电信号，继而转换为数字信号。不同类型的摄像机有不同的原理，可以提供不同的信息。RGB相机是人们日常生活中使用最多的一种相机，其原理是通过红、绿、蓝3种颜色及其组合来获取各种可见颜色。多光谱相机能够获取不同波段的图像，包括可见光和不可见光波长，因此可以获得一些RGB相机无法提供的信息。深度相机则将距离信息加入到了二维图像中，实现了立体成像。视觉传感器移动机器人常用传感器概述触觉传感器使机器人具备了靠触觉来感知的能力。根据原理不同，触觉传感器主要有4种方式。压电式、压阻式、电容式和光学原理式。压电触觉传感器是基于压电效应原理，即在外界力的作用下，压电材料表面因形变会产生电压。它的频率响应好，测量范围大，但分辨率不是很理想。压阻式触觉传感器基于压阻效应原理，即施加外力时会产生自身电阻的变化。它测量范围大，鲁棒性好，但是迟滞效应较大。电容式触觉传感器利用电容的变化来测量接触力，其空间分辨率高，功耗低，但抗干扰能力差。光学原理的触觉传感器靠检测光的参数变化间接感知外界的接触信息。优点是抗干扰能力强，具有很高的空间分辨率。触觉传感器移动机器人常用传感器概述激光发明于20世纪，因为其在单色性、方向性和亮度方面都有出色的性能，因此被广泛应用于各种场合。激光传感器主要由测量电路、激光器和光电探测器等组成。激光器分为四类：固体、液体、气体和半导体。激光传感器主要用于对距离、速度和振动等物理参数的测量上。常见的有激光测距仪、激光位移传感器、激光扫描仪、激光跟踪器等。激光测距的基本原理主要包括三种：飞行时间（TOF）、三角测量法和光学干涉法。TOF是指从发射激光到接收到反射光的总时间。在激光测距仪中，由于光速太高，测距精度取决于飞行时间的测量精度上。三角剖分法利用三角形理论和三角函数来计算物体之间的距离，激光位移传感器就是基于这种方法来实现短距离测量的。两束相位不同的光束叠加后形成明暗条纹的现象被称为光的干涉。此原理被用于激光跟踪器中，可以测量装有反射镜的目标的移动距离。激光传感器能够遥感测量，测量速度和精度都令人满意。但激光波长容易受温度、大气压力和空气湿度变化的影响。激光传感器移动机器人常用传感器概述编码器编码器将角位移或角速度转换为电脉冲或数字量。编码器根据检测原理可分为：光电式、磁性、电感式和电容式。光电式编码器是其中最常用的。将光信号转变为电信号。根据码盘的校准方式，光电编码器分为增量式和绝对式。增量式光电编码器的输出是一系列方波脉冲。旋转角度可以通过记录脉冲的数量计算出来。但是需要一个参考位置作为转轴的零点绝对位置。绝对式光电编码器轴上的每个位置都对应有唯一的二进制数字量，因此可以直接得到绝对位置。除上述4种传感器外，工业机器人中还部署了一些传感器来实现多种功能，如接近传感器、惯性传感器、扭矩传感器、声传感器、磁传感器、超声波传感器等。移动机器人常用传感器概述接近传感器：检测到物体是否接近，并输出相应的开关信号。根据操作原理可分为电容式、电感式和光电式。惯性传感器：包括加速度计、陀螺仪和磁强计。惯性传感器被广泛用于测量运动物体的运动参数，如加速度、角速度和方位角。扭矩传感器：主要用于测量施加在机械轴上的扭矩。常见的类型有感应式和电阻应变式。声传感器：将声波转换成电信号。其中装有电容驻极体传声器，声波会引起传声器中驻极体膜的振动，产生微弱的电压变化，然后对电压进行后续处理。磁传感器：主要用于检测磁场强度，其原理是霍尔效应。超声波传感器：常用于探测障碍物，根据从发射超声波到探测回波的时间来估计物体的范围。移动机器人常用传感器概述惯性导航系统（Inertianavigationsystem，INS）利用陀螺仪和加速度计，同时测量机器人运动的角速度和线加速度，通过导航计算机实时解算移动机器人的三维姿态、速度及位置等导航信息。与其他导航系统相比，INS具有信息全面、完全自主、无源，且不受时间、地点及人为因素干扰等重要特征。在最近几次高技术局部战争中，在具有强磁干扰的极端环境下，仅有INS能够持续地、稳定地正常工作，进一步突出了INS在武器装备中的不可替代性。除了军事用途以外，INS在民用领域也有大量的应用，如无人机、机器人、石油钻井、智能交通及医疗设备等。惯性器件是INS的核心部件，主要包括陀螺仪和加速度计。陀螺仪：主要包括静电陀螺、三浮陀螺、激光陀螺、光纤陀螺、微机械陀螺（MEMS）以及半球谐振陀螺。加速度计：主要包括摆式积分陀螺加速度计、挠性摆式加速度计、石英振梁加速度计、MEMS加速度计以及原子加速度计。移动机器人惯性导航系统MEMS陀螺和MEMS加速度计统称为MEMS传感器，采用MEMS技术研制的陀螺和加速度计，具有体积小、可靠性高、成本低以及易于批量生产等优点，具有重大的战略意义，非常适用于构建轻小型INS。随着MEMS传感器的性能逐步提高，基于MEMS的INS因具有轻小型、低功耗以及低成本等优点，大大的拓展了INS的应用范围。与此同时，军用领域和民用领域对INS的极大需求成为了基于MEMS的INS发展动力。随着集成电路工艺、微电子技术以及纳米技术等技术的不断发展，未来轻小型INS的发展方向为：在同一微小芯片中，集成微小惯性测量单元，实现传感器与处理器一体封装，构成芯片级的超小型INS。轻小型INS的研究方向可以归纳为：(1)不断挑战精度极限(2)组合导航系统研究(3)进一步拓展应用范围移动机器人惯性导航系统航迹推算定位航迹推算是一种使用最广泛的定位手段。航迹推算仅仅需要知道移动机器人本身位移变化就可以实现对移动机器人位置和方向的估计，而不需要外部信息，短期内可以实现高精度定位。航迹推算定位思想就是在测量出单位时间内移动机器人走过的距离和在这段时间内移动机器人航向的变化后，对这些数据进行数学积分运算，进而求出移动机器人行进的距离和航向的变化，这样就可以求出移动机器人的位置以及姿态。因为航迹推算的误差会随着时间累积，所以不适于长时间的精确定位。移动机器人卫星导航系统GPS定位全球定位系统（GPS）是目前应用最广泛、最成熟的卫星导航系统，主要由空间段、地面段和用户段三部分构成。GPS标称卫星星座由24颗卫星组成，卫星均匀分布在6个轨道平面上，每个轨道面上的4颗卫星呈不均匀分布；24颗卫星中只有21颗为工作卫星，另外3颗为备用卫星。地面段主要由1个主控站、6个监测站和4个注入站组成。用户段主要为GPS接收机，接收机的基本功能是接收GPS卫星信号、解调导航电文、实现距离测量、执行导航结算，为用户提供定时、定位和测速服务。GPS系统的优点：全球地面连续覆盖；精度高、功能多，可以提供三维位置、三维速度和时间信息，可以满足军事的要求；全天候工作，一般不受外界环境的影响；抗干扰性强，保密性好；使用低频信号，穿透性强，隐蔽性好，实际生活中得到了广泛应用。移动机器人卫星导航系统利用单目视觉（monocularvision）系统实时监测本车与前方障碍物的距离，可以为机器人提供输入参数，摄像机拍摄到的场景图像是3维空间的场景在2维空间的投影，而在利用机器视觉对道路情况进行识别的过程中，则需从2维图像还原成路面图像。这个过程就是道路深度信息的获取。目前单目视觉系统一般采用对应点标定法来获取图像的深度信息，对应点标定法是指通过不同坐标系中对应点的对应坐标求解坐标系的转换关系。但对应点标定法在标定过程中，由于受器材限制，仍无法做到十分精确地记录一个点在世界坐标系和图像坐标系中的对应坐标。如果其坐标不够精确，那么得到的转换矩阵的精确度也会受到制约，坐标转换结果的精度也会因此而波动。移动机器人视觉导航系统由于对应点标定法对于摄像机的标定是在摄像机的各个角度及高度已经确定的情况下进行的，当摄像机的任何一个参数发生变化时，都要重新进行标定，以得到在该种具体情况下的转换矩阵，所以该方法仅适用于位置固定的摄像机的情况，而对于应用在智能汽车上的摄像机来说，由于汽车行驶过程中会有颠簸，致使摄像机的参数发生变化，所以适用性受到了极大限制。几何关系推导法较好地解决了对应点标定法的不足。它根据摄像机投影模型，通过几何推导来得到路面坐标系和图像坐标系之间的关系。根据小孔成像模型，可以将单目视觉系统简化为摄像机投影模型，如图1所示。移动机器人视觉导航系统根据小孔成像模型，可以将单目视觉系统简化为摄像机投影模型，如图1所示。移动机器人视觉导航系统图1摄像机投影模型图1中，平面ABU代表路平面，ABCD为摄像机拍摄到的路平面上的梯形区域，O点为摄像机镜头中心点，OG为摄像机光轴，G点为摄像机光轴和路平面的交点（同时也是视野梯形的对角线交点），I点为点在路平面上的垂直投影。在路面坐标系中，将点G定义为坐标系原点，车辆前进方向定义为Y轴方向。G、A、B、C、D各点在图像平面内的对应点如图1右侧子图所示，a、b、g、d为像平面矩形的4个端点，H和W分别为像平面的高和宽。定义图像矩形的中点g为像平面坐标系的坐标原点，y轴代表车辆前进方向。移动机器人视觉导航系统取路平面上一点P，其在路平面坐标系的坐标为(XP，YP)，点P在图像平面内的对应点p为其在像平面坐标系的坐标为(xp，yp)。由几何关系可以推导出如下路面坐标与图像坐标之间的对应关系：(1)其中，(2)式(1)的前两式为像平面到路平面的映射关系；后两式为路平面到像平面的逆映射关系。式(2)中，H为图像的高；W为图像的宽；h为摄像机的安装高度；

为摄像机镜头的水平视野角；

为摄像机镜头的垂直视野角；

为摄像机的俯仰角。移动机器人激光雷达/红外定位系统激光雷达类似于人的眼睛，在自动设备运功过程中对其周围物体的感知显得尤为重要。近年来随着激光雷达技术的逐渐成熟和设备生产工艺的优化，该技术也越来越多的应用于自动驾驶技术、自动机器人等民用行业。激光探测及测距系统简称为激光雷达，也称LaserRadar或LADAR，是NASA于1970年开始研发。近年来随着激光雷达技术的逐渐成熟和设备生产工艺的优化，该技术也越来越多的应用于自动驾驶技术、自动机器人等民用行业。依据激光雷达的线数可分为多线激光雷达和单线激光雷达。对于雷达性能要求不高的服务机器人多采用单线激光雷达，例如餐厅服务机器人、医疗机构内部使用的“物流机器人”等。单线激光雷达为单频点工作，为了取得周围360度的物体位置信息，不得不增加一个机械旋转机构和增加扫描频率，并通过机械旋转速度和扫描频率匹配来获取外部信息。自动驾驶技术中多采用多线激光雷达。基于卡尔曼滤波的运动体姿态估计方法最优估计是在一定准则下使信号的估计达到最优。在20世纪40年代，为了解决火力控制系统精确跟踪问题，维纳提出了维纳滤波理论。但是维纳滤波计算繁琐，卡尔曼于20世纪60年代提出卡尔曼滤波理论，它选取的准则是使状态量的误差方差最小，所以是最小误差方差估计。卡尔曼滤波最大的优点是采用递推形式，实时地计算出结果。最初提出的卡尔曼滤波只适用于线性系统，扩展卡尔曼滤波则可以推广到非线性的状态方程中。虑到运动体的姿态动力学模型是非线性的，故应用针对非线性问题提出的扩展卡尔曼滤波方法对运动体的姿态进行估计。基于卡尔曼滤波的运动体姿态估计方法系统结构如图2所示，通过对三轴陀螺仪、三轴加速度计和三轴磁力计输出的原始数据进行融合，估计出较精准的姿态，从而给控制器提供精确可靠的状态数据，使得运动体能够以更加稳定的姿态执行并顺利完成任务。图2系统结构图滤波器的状态方程与测量方程为：(3)基于卡尔曼滤波的运动体姿态估计方法雅可比线性化和离散化后表示为：(4)式中，状态

为运动体姿态四元数；由于采用加速度计输出a和磁力计输出m的数据来纠正运动体的姿态，所以

；W和V分别是状态噪声和测量噪声，均是服从高斯分布的白噪声，即

、 ,Q和R是和的协方差，且(5)F和H分别是连续时间函数

和

离散化的雅可比矩阵:(6)基于卡尔曼滤波的运动体姿态估计方法式中，

是忽略地球东西方向磁场后根据坐标系得到的修正值，且(7)扩展卡尔曼滤波数据融合算法流程如下第一步，进行状态和协方差预测(8)式中，

是第

时刻状态

的协方差矩阵；

是第

时刻状态

的先验估计值。第二步，进行状态和协方差更新(9)式中，

是卡尔曼增益；

是单位矩阵。移动机器人的多传感器信息融合方法随着移动机器人拥有越来越高的感知能力和强大的信息处理能力，使它具有越来越多的功能并且可以完成不同的任务。移动机器人的感知能力类似于人体的感官系统，为了准确地感知事物，必须多个器官相互配合，才能完整地感知整个环境信息。移动机器人感知环境也需要多个或多种传感器才能更完整地感知环境。因此，提出了多传感器信息融合技术的问题，这也是移动机器人感知环境方法研究的难点。多传感器信息融合技术多传感器信息融合是一种信息处理方法，旨在解决在系统中使用多个或多种传感器的问题。通过集成多类同构的冗余信息或异构传感器的互补信息，可以更准确地评估观察对象，从而做出正确的判断和决策。与单个传感器相比，多传感器信息融合可提高系统的检测性能，扩大时空覆盖范围，提高系统的可靠性和可维护性，提高容错性和鲁棒性。移动机器人的多传感器信息融合方法多传感器信息融合原理多传感器信息融合基本原理是充分利用多传感器资源，通过合理控制和使用多传感器及其观测信息，按照一定的标准，将多传感器的冗余或互补信息在空间或时间上进行组合，从而获得一致的信息、被测物体的解释或描述。具体原理如下：①种不同类型的传感器收集观测目标的数据；②对传感器输出数据（离散或连续时间函数数据，输出矢量，成像数据或直接属性描述）进行变换，以提取表示观测数据的特征矢量；③通过模式识别对特征向量进行处理，以完成对每个传感器的描述；④将每个传感器的目标描述数据按照同一目标进行分组，即关联；⑤融合算法用于合成每个目标的传感器数据，以获得对目标的一致解释和描述。移动机器人的多传感器信息融合方法多传感器信息融合的具体过程如图3所示，主要包括五个步骤：数据采集，数模转换，数据校验，信息分类和融合处理。图3

融合流程图多传感信息融合分类多传感器融合在结构上按其在融合系统中信息处理的抽象程度，主要划分为三个层次：数据层融合、特征层融合和决策层融合。数据层融合是最低级别的融合，具体过程为将传感器测量的原始数据可以精确到单个值。由于这种原始数据尚未处理，因此融合级别具有最大程度地保留原始数据详细信息的能力。但是，该方法计算量大，抗干扰能力差。传感器的性能和状态对这种数据融合有很大的影响。移动机器人的多传感器信息融合方法特征层融合是中间级别的融合，具体过程是从传感器测量的原始数据中提取特征，保留有效信息，消除噪声和冗余信息，以及然后对这些特征进行分类和匹配，以使这些特征可以一对一地对应于相应的传感器和被观察物体。该方法同时考虑了数据级融合和决策级融合，因此在抗干扰和细节保存方面取得了良好的效果。决策层融合是最高级别的融合，具体过程是在检测到每个传感器的数据之后，得出一致的最优值。决策层融合方法灵活性高，对传感器性能的依赖性较小，抗干扰能力强，但会造成严重的信息丢失，无法获取详细信息，融合前需要进行大量的数据预处理。移动机器人的多传感器信息融合方法多传感信息融合方法由于传感器信息在融合过程中将以不同种形式表现出来，从而增加了多传感器信息融合的复杂性和多样性，因此多传感器信息融合的研究必须涉及许多基础理论领域。常用的多传感器信息融合方法如下所示。①贝叶斯估计通过最大似然估计函数，剔除传感器中无用和偏差大的观测数据，然后进行融合。②模糊理论通过模糊集合和模糊隶属度将传感器信息中的不确定性转换到0-1上，适用于系统不确定性建模，并且模糊理论常常和神经网络结合，但模糊理论缺点在于会损失系统的精度。移动机器人的多传感器信息融合方法③D-S证据推理D-S证据推理是不确定推理方法的一种，首先得到各个传感器的信任函数，之后通过D-S组合原则对这些数据进行融合，并据此来判断目标特征和决策，D-S证据推理很好地结合了主观不确定信息。④卡尔曼滤波理论一种递推式的最优估计理论，建立测量和估计方程来实现递推式计算。一般卡尔曼滤波用在数据融合较低层面上，直接对传感器信号进行处理，有效去除干扰信息。⑤神经网络理论神经网络是当前应用十分广泛和流行的理论，其通过建立仿人神经结构的网络，实现数据的非线性映射能力，尤其在融合系统没有函数模型的情况下，其通过测试数据的大量训练得到网络结构和映射关系，并具有良好的自适应性，十分适合复杂多传感器数据融合的场景。移动机器人的多传感器信息融合方法多传感器信息融合在移动机器人方面的应用多传感器信息融合已经广泛应用在移动机器人。通过大量实验证明，采用单一传感器移动机器人和使用多传感器的移动机器人在感知外部环境的能力方面是不可比拟的。1979年，由法国LASS实验室研制移动机器人HILARE成为第一个将多传感器信息融合技术用于检测复杂环境信息的移动机器人。1997年，由美国国家航空航天局开发的火星探测机器人Sojourner降落在火星上。该机器人采用扩展的卡尔曼滤波器，并且融合了视觉传感器、速度传感器、加速度传感器、里程表和航向传感器的信息。2007年，北京理工大学的“汇童”仿人机器人融合了力觉、视觉以及平衡觉等，不仅能够在不确定环境稳定运行还可以实现打乒兵球等智能活动。2018年，北京建筑大学采用激光测距仪和里程表的数据融合形式，通过扩展卡尔曼滤波算法对两种传感器测得的距离数据进行融合计算，校正里程表数据，减少里程表测量过程中的累积误差，获得移动机器人在运动过程中的精确位置信息。提高移动机器人导航精度的方法机器人导航技术背景除工业机器人外，几乎所有智能机器人都离不开行走，所以机器人导航技术是智能机器人的核心技术之一。针对导航技术的研究在不断地选代更新。自主定位和导航是智能机器人研究领域的一项核心技术，地图构建、自主定位和路径规划等是需要解决的关键问题，也就是机器人“我在哪里”“我要去哪里”“怎样到达”3个问题。机器人需要通过传感器感知环境的信息，获取环境数据并结合自身的状态最终做出适合于环境和目标的决策。而其中，机器人的定位问题是最基本的问题，只有解决了定位问题，才有可能解决“去哪里”“怎样到达”的问题。提高移动机器人导航精度的方法机器人的导航方式概述机器人在利用导航技术获取周边环境信息以确定行走方向或所在位置时，需综合利用科学算法，得出数据模型并形成指令，方可规划最优路径。目前机器人常用的定位导航方式有电磁导航、惯性导航、视觉导航和激光导航等。①电磁导航电磁导航是最早被研发的导航技术。技术人员在机器人预设路线上设置多条电线，机器人在运行中通过接收电流频率，通过感应线圈感知行走指令。经过多年尝试，科研人员认定该方法准确率高且运用简单，但前期铺设电线需消耗过多人力、物力，因此该方法的维护成本较高。提高移动机器人导航精度的方法②惯性导航惯性导航也是移动机器人上较为常见的一种导航技术，惯性导航主要通过自身携带的陀螺仪进行机器人行动方向的检测，并以此完成对移动机器人的导航，不同于电磁导航的较高成本，惯性导航的成本较为亲民，这也使得惯性导航在我国当下移动机器人中有较为广泛的应用，不过由于这一技术只能满足移动机器人短时间的移动需要这就使得惯性导航技术的应用范围较为有限。③视觉导航视觉导航是机器人常用的导航技术，其特点是探测信号范围广、信息获取准度高。研究人员在移动机器人头部安装巡航影像系统，其内部含有传感器辨识系统、图像辨别系统和路径规划技术等。视觉导航采用电荷耦合器件图像传感器（CCD），它是集信息划分、整理、读取为一体的综合传感器，当机器人进行路径规划时，CCD传感器会将传输至系统原件的电荷转移至影像系统内的二维传感器进行合成，通过系统扫描对数据进行量化建模，形成指令。提高移动机器人导航精度的方法④激光导航激光雷达+SLAM技术相结合的激光定位导航主要是通过对目标物发射激光信号，再根据从物体反射回来的信号时间差来计算这段距离，然后再发射激光的角度来确定物体和发射器的角度，从而得出物体与发射器的相对位置。目前激光定位导航技术已成为机器人主流的定位导航方案。激光雷达具有指向性强的特点，使得导航的精度得到有效保障，能很好地适应室内环境。⑤其他导航移动机器人除以上定位导航技术外，红外线定位导航、超声波定位导航、GPS定位导航等也是其比较常用的自主定位导航技术。提高移动机器人导航精度的方法激光导航的工作原理是通过激光获取环境信息，测量激光从发出到接收的时间计算出自身距离前方障碍物的距离，通过算法的处理，同样得出环境模型，在不断的扫描测距中获得定位和行走路线。激光雷达作为移动机器人技术中的后起之秀，因为成本高昂而起步较慢，但随着低成本激光雷达的研发，激光雷达导航的性能更加优异，激光雷达具有高精度、高分辨率的优势，在地图的精度和定位上更精准。在机器人机身上方安装了激光雷达，搭载SLAM的核心算法，可以让移动机器人快速感受全局环境，实现分秒建图以及实时定位，从而实现智能导航和路径规划，完全摆脱人工的辅助，激光雷达技术在不断的精进中，价格趋于合理对于智能机器在地图、路径上的智能化程度更高，随着技术的进步可以预见激光雷达技术有着更广阔的前景。激光导航技术原理及现状提高移动机器人导航精度的方法惯性测量单元（inertialmeasurementunit，IMU）是测量物体三轴姿态角（或角速率）以及加速度的装置。一般1个IMU内会装有3轴的陀螺仪和3个方向的加速度计，来测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。为了提高可靠性，还可以为每个轴配备更多的传感器。一般而言，IMU要安装在被测物体的重心上。值得注意的是，IMU提供的是一个相对的定位信息，它的作用是测量相对于起点物体所运动的路线，所以它并不能提供物体所在的具体位置的信息，且惯导随着时间变化误差会不断积累，所以这个方法在移动机器人导航中往往不会单独使用，而是和其他技术一起进行融合。由其他的绝对定位技术提供校正信息，而惯导的信息提供连续的定位和起到对其他位置的平滑作用。目前，IMU正在向高性能、小体积、低成本的方向不断进步。IMU技术原理与现状提高移动机器人导航精度的方法目前的室内智能机器人普遍使用激光测距传感器进行定位，位姿信息可以使用二维扫描匹配算法获得，其中最典型的是迭代最近点ICP（iterativeclosestpoint）算法。对于两组激光点云数据（参考点云和待匹配点云），标准的ICP算法分为4步：①对于待匹配点云，搜索每个点在参考点云中的最近点，形成对应点集；②求解配准变换，使所有对应点对的距离平方和最小；③对待匹配点云应用该配准变换；④重复以上步骤，直至计算结果收敛到设定精度或达到设定的选代次数为止。定位算法技术现状提高移动机器人导航精度的方法该方法在理论上可收敛到一个局部极值，但在实际应用过程中，由于激光传感器自身的离散测量特性，实际上所有对应点之间的关联都是近似的，这样ICP算法的收敛速度和精度无法得到保证，因此基于该算法，延伸出了多种改进方法，比如变种PL-ICP（pointtolineICP）等扫描匹配算法在实际中也获得广泛应用，它具有二阶收敛性，且可在有限步数中收敛，但对于较大的旋转运动鲁棒性较弱。由于该缺点的存在，机器人在使用过程中经常在旋转速度较大时匹配无法收敛，从而丢失定位，无法实现正常的定位导航功能。提高移动机器人导航精度的方法多传感器融合在移动机器人导航中的应用现阶段，移动机器人无轨导航主要依靠激光雷达和摄像头等类型的光学传感器，传感器扫描空间的形状，通过计算得到移动机器人的位置，也就是即时定位与地图构建(simultaneouslocalizationandmapping，SLAM)。这类传感器在使用中扫描周围环境后测得的位姿信息存在误差，周围环境也存在随时变化的可能，导致在需要精确定位的区域，移动机器人的定位精度较低；在某些特殊需要精确定位的位置，SLAM算法下的定位精度不能满足要求，此时需要采取额外措施实现精确定位的要求。多传感器信息融合技术是智能移动机器人的关键技术之一。多传感器信息融合技术能够有效地利用多传感器信息，克服信息的不完备性和不确定性；能够更加准确、全面地认识和描述被测对象，从而做出正确的判断与决策，解决单一传感器定位精度不足的问题。飞行机器人智能感知8.2目前，飞行机器人技术发展十分迅速，飞行性能也正在逐步完善，并朝着微型化、智能化的方向发展。飞行机器人质量轻、成本低、体积小、能够在复杂多变的环境中灵活飞行，在生活和生产中发挥着至关重要的作用。在其智能化进程中，智能感知是非常重要的技术，只有感知和测量出周围环境，才能使飞行机器人的飞行更加稳定、精准。感知所使用的基础元件是传感器，灵敏、精确的传感器可以更好估计出运动状态，提高飞行机器人导航精度。飞行器的传感器标定和测量模型1.三轴加速度计(1)三轴加速度计当物体具有很大的加速度时，物体及其所载的仪器设备和其他无相对加速度的物体均受到能产生同样大的加速度的力，即受到动载荷，想要知道动载荷就要测出加速度。而加速度计就是一种能够测量运载体线加速度的仪表，由检测质量、支承、电位器、弹簧、阻尼器和壳体组成。加速度计按实现方式，可分为压电式、电容式及热感应式三种；按输入轴数目分类，可分为单轴、双轴和三轴加速度计。其中，三轴加速度计是加速度传感器中用来测量空间加速度的传感器，即测量物体在空间中速度变化的快慢。三轴加速度计与单轴、两轴加速度计在测量原理上没有差别，它们的主要差别在于测量的维度不同。三轴加速度计的优点在于可以在预先不知道物体运动方向的场合下，用来测量加速度信号，能够全面准确反映物体的运动性质，且其体积小、重量轻，因此被作为惯性导航系统的基本组成元件，广泛应用于航空领域之中。飞行器的传感器标定和测量模型(2)标定三轴加速度计的常见指标值有：量程、分辨率、零偏、刻度系数、交叉耦合系数、刻度系数的非线性度、零偏稳定性、零偏重复性、带宽、温度等。其误差的产生来源于多方面因素，主要包括：量化噪声、加速度随机游走、角速率随机游走、零偏不稳定性噪声、速率斜坡、零偏重复性等。通常情况下，需要对三轴加速度计的刻度系数矩阵和零偏进行标定。进行标定时需要使用转台，以重力场作为参考，求解其刻度系数矩阵和加速度计的零偏。常采用六面法对三轴加速度计进行标定，即依次将三个加速度计朝天和朝地放置，并采集静止状态下三个加速度计的输出，以重力加速度为参考基准值，将不同位置时三个加速度计的输出值和相应的重力加速度向量代入三轴加速度计的标定模型。飞行器的传感器标定和测量模型(2)标定常用的三轴加速度计标定模型：飞行器的传感器标定和测量模型（10）式中，为加速度计的输出值，

为加速度计的零偏，为刻度系数矩阵，包含了三个轴的刻度系数和六个交叉耦合系数，为标定基准值，一般取重力加速度。使用最小二乘法即可计算加速度计的刻度系数矩阵和零偏，完成加速度计标定。除了常用的六面法以外，还有其他方法可以对加速度计进行标定，比如基于卡尔曼滤波的标定、基于多位置的快速自标定等。(3)测量模型加速度计中的检测质量受支承的约束只能沿一条轴线移动，这个轴常称为输入轴或敏感轴。当仪表壳体随着运载体沿输入轴方向作加速运动时，根据牛顿定律，具有一定惯性的检测质量会保持其原来的运动状态不变，因此它与壳体之间将产生相对运动，使弹簧变形，于是检测质量在弹簧力的作用下随之加速运动。当弹簧力与检测质量加速运动时产生的惯性力相平衡时，检测质量与壳体之间便不再有相对运动，这时弹簧的变形反映被测加速度的大小。电位器作为位移传感元件把加速度信号转换为电信号，最后通过ADC转换器转换为数字信号从而测出加速度的大小。三轴加速度计的实现原理与单轴类似，是将空间加速度在X、Y、Z三个敏感轴上进行分解，这样，三轴就可以变成三个单轴进行测量。本质上，三轴加速度计是一个三自由度的震荡系统，需要采用阻尼器来改善系统的动态品质。飞行器的传感器标定和测量模型2.三轴陀螺仪(1)三轴陀螺仪陀螺仪可对自己的运动状态和方向进行判断，主要由陀螺转子、内外框架和附件组成。陀螺转子通常采用同步电机、磁滞电机、三相交流电机等拖动方法来使陀螺转子绕自转轴高速旋转；内、外框架一般都是环形，可以使陀螺自转轴获得所需角转动自由度；附件一般包括力矩马达、信号传感器等。陀螺仪按实现方式，可分为压电式、电容式及热感应式三种；按功能特点分类，可分为激光陀螺仪、MEMS陀螺仪、光纤陀螺仪等；按输入轴数目分类，可分为单轴、三轴和多轴陀螺仪。陀螺仪测量的物理量是偏转、倾斜时的转动角速度，然后判别物体的运动状态，为运动指定方向。三轴陀螺仪可以同时测定六个方向的位置、移动轨迹、加速度，而单轴的只能测量一个方向的量。因此，一个三轴陀螺仪就能替代三个单轴陀螺仪，所以三轴陀螺仪相对于单轴而言，体积小、重量轻、结构简单、可靠性好，有着更为广泛的用途。飞行器的传感器标定和测量模型(2)标定三轴陀螺仪和三轴加速度计都是惯性导航和惯性制导系统的基本测量元件，它们有着许多相似之处，常见的指标值也都彼此类似。对三轴陀螺仪进行标定时需要使用转台，标定的参数一般有：刻度系数矩阵和零偏。刻度系数是指陀螺的输出物理量与输入角速率的比值。零偏是指当输入角速率为零时，陀螺的输出值。若是对微机械陀螺这一类受温度影响较大的器件进行标定，则需要考虑实际的应用场景，因为其零偏和刻度系数会随着温度的变化发生改变，要对温度的影响进行合理的补偿。飞行器的传感器标定和测量模型(2)标定进行标定时，将陀螺仪固定安装在转台上，依次让陀螺仪的三个轴与转台的转轴平行，其余两个轴与转台的转轴垂直，让转台按照设定的转速旋转，得到转台在不同角速率下旋转时三个陀螺的采样输出值，同时记录转台的实际转速作为标定时的基准值。在解算的过程中，为了降低数据采集过程中噪声的影响，可以计算在各个角速率下陀螺输出的均值，将计算的各个均值与记录的转台的实际转速代入式(10)中，用最小二乘法即可求解得到陀螺的零偏和刻度系数矩阵，完成陀螺的标定。在标定过程中可以将陀螺先开机几分钟，待产品输出的数据达到稳定后再采集数据进行标定，且采集数据的过程中，应在转台转速稳定后同时采集并保存三个陀螺的输出值。飞行器的传感器标定和测量模型(3)测量模型陀螺仪的陀螺装置由陀螺部分和电源部分组成，其工作原理是高速回转体的动量矩敏感壳体会相对惯性空间绕正交于自转轴的一个或二个轴进行角运动。因此在不加外力时，空间任意固定点的系统的全角运动量不变，且绕任意轴旋转的角运动量变化率都等于使该轴旋转的转矩。根据这个性质可知，高速旋转的物体的旋转轴，对于改变其方向的外力作用有趋向于垂直方向的倾向，而且旋转物体在横向倾斜时，重力会向增加倾斜的方向作用，而轴则向垂直方向运动，也称为岁差运动。飞行器的传感器标定和测量模型3.三轴磁力计(1)三轴磁力计磁力计也叫地磁、磁感器，主要用于测试磁场的强度和方向，定位设备的方位，其原理跟指南针原理类似，可以测量出当前设备与东南西北四个方向的夹角。三轴磁力计由：各项异性磁阻材料、电极、氧化物沟槽几个部分组成。由于磁感应强度是矢量，具有大小和方向，所以只测量磁感应强度大小的磁力计称为标量磁力计，而能够测量特定方向磁场大小的磁力计称为矢量磁力计。常见标量磁强计有质子旋进磁力计、Overhauser磁力计，碱金属光泵磁力计等；常见的矢量磁力计有磁通门磁力计、磁阻磁力计等。三轴磁力计有许多优势，比如：输出具有连续性、误差不积累、成本低、性能好。在导航系统中，磁力计是用于姿态估计的关键性辅助传感器，广泛应用于低成本飞行器的组合导航系统中。飞行器的传感器标定和测量模型(2)标定三轴磁力计的误差主要来源于自身内部、安装误差和罗差。自身内部误差主要包括：零位误差、灵敏度误差和正交误差等；安装误差是由于磁力计安装于载体上时没有实现磁力计的坐标系与载体坐标系相互重合产生的。因为铁磁材料本身的特性会对外界磁场产生影响，因此当它出现在磁力计周围时必然会对其产生影响进而引起误差，我们把这种误差称之为罗差。在实际使用中，三轴磁力计都会存在未对准、灵敏度不一致和零偏等误差，导致测量值误差过大。因此必须对三轴磁力计进行标定之后，才能将三轴磁力计用于组合导航系统中。飞行器的传感器标定和测量模型(2)标定通常情况下，现有的三轴磁力计标定方法大都是对采样数据进行归一化，然后利用样本数据求解参数。除此之外，也有通用性和有效性更好的基于模值估计的三轴磁力计标定方法。这是一种改进的标定方法，首先将磁力计所处位置磁场向量的模作为误差模型参数，然后对其进行估计算法设计，接着利用两步标定方法进行参数初值的选取，用Levenberg-Marquardt方法进行磁力计标定算法设计。这种算法对类似的三轴传感器具有通用性，不依赖于传感器的观测向量，同时能够对所观测向量的模值进行精确的估计，可以标定三轴磁力计的安装误差、静态灵敏度误差和零位误差，标定过程简便，适用性广。飞行器的传感器标定和测量模型(3)测量模型磁力计内部采用各向异性磁致电阻材料来检测空间中磁感应强度的大小。这种具有晶体结构的合金材料对外界的磁场很敏感，磁场的强弱变化会导致各向异性磁致电阻自身的电阻值发生变化。当有外界磁场时，各向异性磁致电阻上主磁域方向就会发生变化，而不再是初始的方向，因此磁场方向和电流的夹角也会发生变化。对于各向异性磁致电阻材料来说，夹角的变化会引起各向异性磁致电阻自身阻值的变化，并且呈线性关系。利用电桥检测各向异性磁致电阻阻值的变化。这样在没有外界磁场的情况下，电桥的输出为零；而在有外界磁场时电桥的输出为一个微小的电压，由此可以测量出磁场的方向和大小并被量化表示。飞行器的传感器标定和测量模型4.超声波测距仪(1)超声波振动在弹性介质内的传播就成为波动，简称波。频率在Hz之间的机械波可以被人耳所闻，称为声波；低于20Hz的机械波称为次声波；高于Hz的机械波称为超声波；在Hz之间的波称为微波。超声波在液体、固体中衰减很小，穿透能力强，特别是对不透光的固体，超声波能穿透几十米的厚度。但当超声波由一种介质入射到另一种介质时，由于在两种介质中传播速度不同，在介质面上会产生反射、折射和波形转换等现象。所以，利用超声波的这些性质制作了敏感器件。飞行器的传感器标定和测量模型(2)超声波测距仪能够产生超声波、接受超声波，并且以超声波作为检测距离的手段的装置就是超声波测距仪。主要利用超声波测距仪中的探头发射和接收超声波，发射探头发出的超声波脉冲在介质中传到相介面经过反射后，再返回到接收探头。由于声源与目标之间的距离和声波在声源与目标之间往返传播需要的时间成正比，所以可以通过测量声波往返的时间求出距离。超声波探头常用的材料是双面压电晶体或压电陶瓷，这种探头被统称为压电式超声波探头，是利用压电材料的压电效应来工作的。在压电晶体上加有大小和方向不断变化的变流电压时，就会使压电晶体产生机械变形，这种机械变形的大小和方向与外加电压的大小和方向成正比。飞行器的传感器标定和测量模型(2)超声波测距仪压电晶体上加有一定频率的交流电压时，其也会产生同频率的机械振动，这种机械振动推动空气等媒介，便会发出声波。压电效应具有可逆性，逆压电效应是将高频电脉冲转换成高频机械振动，以产生超声波，可作为发射探头。正压电效应是将高频机械震动转换成高频电脉冲，可以接收超声波信号，作为接收探头。由于压电晶体有一个固有谐振频率，即中心频率，所以在发射声波时，加在其上面的交变电压频率要与它固有的谐振频率相同；接收超声波时，作用在其上面的超声机械波的频率也要和固有谐振频率相同。这样才能有较高的灵敏度。飞行器的传感器标定和测量模型(3)标定一般而言，最常用的标定方法就是使用测距仪对测距仪进行标定。对标定测距仪的选择需要根据被标定的测距仪测量精度而定，其基本原则是：用来标定的测距仪精度一定要高于被标定测距仪一个量级。所以，对于超声波测距仪而言，使用激光测距仪就可以进行标定。(4)

测量模型超声波测距仪主要由超声波发射器、超声波接收器、定时电路、控制电路、锥形辐射喇叭、金属网及金属壳组成。待超声波发射器发出脉冲式超声波，然后关闭发射器的同时打开超声波接收器。该脉冲到达物体表面后返回到接收器，定时电路测出从发射器发射到接收器接受的总时间。

飞行器的传感器标定和测量模型(4)

测量模型超声波测距仪有两种工作模式，即对置模式和回波模式。在对置模式中，接收器放置在发射器对面；在回波模式中，接收器和发射器集成在一起。若接收器在其工作范围内或声波被靠近传感器的物体表面反射，则接收器就会检测出声波，并会产生相应的信号；否则，接收器就接收不到声波，也就不会产生信号。若时间设为

，而声波的速度为

，则被测距离为

，即若为对置模式，则超声波的传播速度与其波长和频率成比例，

，若波长和频率不变，则速度为常速。在真实的测量环境中存在一些不确定性，如声波在空气中的传播速度受温度影响很大，和湿度也有一定关系。随着环境温度和湿度的变化，波速就会发生变化，测出的距离就会产生误差；在复杂环境中，如拐角处可能会产生镜面反射，导致无法接收到回波，便无法产生测量的距离。

飞行器的传感器标定和测量模型(11)(12)5.气压计(1)气压计气压计是可以测量大气压强的仪器，通过对大气压强的测量，可以测出飞行机器人当前的飞行高度。除此之外，还可以预测天气的变化，气压高时天气晴朗；气压降低时，将有风雨天气出现。根据气压计结构的不同，其种类可以分为水银气压计及无液气压计，都是依据托里拆利实验的原理制作的。(2)标定由于气压计的准确性容易受到温度和湿度等一些不确定性的影响，为了确保气压计能够准确的测出气压值，需要经常进行标定。为了能够测量出相对参照地的实际飞行高度，需要把参考地的气压数字输进气压计中，通过与参考地的气压进行校准，才能准确测量出相对飞行高度。通常以海平面处的气压值作为参考气压值，这样测出的高度即为相对于海平面的绝对高度。飞行器的传感器标定和测量模型(3)测量模型气压计的测量模型以托里拆利实验为基础，可以把大气压用水银柱的高度体现出来。用试管装满水银排出管中的空气，然后将试管倒置在装有水银的水槽中，此时水银槽内水银面的压强和管内与水银槽内水银面等平面的压强相等，而管内的压强由管内水银柱产生，所以管内水银柱的压强等于大气压强。水银气压计玻璃管底部的水银槽是用一个皮囊所代替，并附有可以调准的象牙针使其指示水银面，叫做“福廷式水银气压计”。飞行器的传感器标定和测量模型(3)测量模型在玻璃管外面加上一个金属护套，套管上刻有量度水银柱高度的刻度尺。在水银槽顶上另装一只象牙针，针尖正好位于管外刻度尺的零点，另用皮袋作为水银槽底。使用时，轻转皮袋下的螺旋，使槽内水银面恰好跟象牙针尖接触（即与刻度尺的零点在一水平线上），然后由管上刻度尺读出水银柱的高度，每升高12米，水银柱即降低大约1毫米，因此可测山的高度及飞机在空中飞行时的高度。无液气压计也是气压计的一种,最常见的是金属盒气压计。它的主要部分是一种波纹状表面的真空金属盒。为了不使金属盒被大气压所压扁，用弹性钢片向外拉着它。大气压增加，盒盖凹进去一些；大气压减小，弹性钢片就把盒盖拉起来一些。盒盖的变化通过传动机构传给指针，使指针偏转。从指针下面刻度盘上的读数，可知道当时大气压的值。它使用方便，便于携带，但测量结果不够准确。若在无液气压计的刻度盘上标的不是大气压的值而是高度值，那么就变成了航空用的高度计。飞行器的传感器标定和测量模型6.二维激光测距仪(1)激光激光诞生于20世纪60年代初，它与普通光相比，具有方向性好、亮度高、颜色极纯、相干性好等特点。激光在空间是定向传播，光束的发散度极小，发射角比较小，且发光光谱很纯，单色性好，称得上比较严格的平行光束。激光的发射是大量光子集中在一个极小的空间内向一个固定的方向射出，光子能量密度很高，所以激光的亮度极高。围绕着激光的使用和研究，激光技术作为一种新的技术也孕育而生。经过60多年的研究，在社会生活许多方面的应用已经取得了进展。例如：军事领域激光主要从事测距、制导、侦察和通信；在医学方面，激光技术已成为临床治疗的有效手段；在工农业生产方面，激光可用于工业材料加工及种子优化改良。飞行器的传感器标定和测量模型(2)二维激光测距仪激光测距仪是利用调制激光的某个参数实现对目标的距离测量的仪器。按照测距方法分为相位法测距仪和脉冲法测距仪；按照维度数目不同，可以分为一维激光测距仪、二维激光测距仪和三维激光测距仪。一维激光测距仪用于距离测量、定位；二维激光测距仪用于轮廓测量、定位、区域监控等领域；三维激光测距仪用于三维轮廓测量、三维空间定位等领域。激光测距仪重量轻、体积小、操作简单、速度快而准确，其误差仅为其它光学测距仪的五分之一到数百分之一。二维激光测距仪由激光发射器、接收器、钟频振荡器及距离计数器等组成，可以通过发射和接收激光，并进行计时，计算出距离。二维激光测距仪还能用来对人造卫星跟踪测距，测量飞机飞行高度，对目标进行瞄准测距，以及进行地形测绘，勘察等。飞行器的传感器标定和测量模型(3)标定在对二维激光测距仪的标定中可采用最小解外参数法对位姿进行标定。用最小二乘把激光测距仪内观测矢量、系数矩阵中包含误差的元素重新构成未知矢量，将最小解当作位姿标定初值，让数据转换成目标坐标信息；创建摄像机透视投影模型，引入透镜的径向畸变和切向畸变，利用标定板把激光束简化为单点激光，采用主成分分析法优化各点到直线的垂直距离，减少点到直线的投影误差，获得准确的二维激光测距仪位姿标定结果。二维激光测距仪阵列可以选取特殊物体来进行标定，其中墙体是最有利的。在二维激光测距仪采集的数据中可以轻易的找到墙体的数据，其数据是一条排成直线的激光点，从而依据二维激光测距仪在平面标定板上形成的点云数据，由最小二乘法进行直线拟合求出其平移向量便完成了标定。也可基于虚拟二面体来进行多个二维激光测距仪阵列标定。首先标定两组二维激光测距仪，计算两组二维激光测距仪之间的空间变换关系，根据它们之间的变化关系来求出全部的二维激光测距仪之间的标定。飞行器的传感器标定和测量模型(4)测量模型脉冲式激光测距仪是在工作时向目标射出一束或一序列短暂的脉冲激光束，由光电元件接收目标反射的激光束，计时器测定激光束从发射到接收的时间，计算出从观测者到目标的距离。相位法激光测距仪是利用检测发射光和反射光在空间中传播时发生的相位差来检测距离的。测距原理基本可以归结为测量光往返目标所需要时间，然后通过光速和大气折射系数计算出距离。需要注意，测量相位并不是测量激光的相位，而是测量调制在激光上面的信号相位。在对物体平面进行测量时必须与光线垂直，这样光反射回来的信号强度够大。否则返回信号过于微弱将无法得到精确距离。飞行器的传感器标定和测量模型7.全球定位系统(1)全球定位系统全球定位系统（GlobePositionSystem）简称GPS,是一个中距离圆型轨道卫星高精度导航与定位系统。该系统由24颗卫星组成，分布在六个轨道面上，卫星距地面大约两万千米，绕地球一周时间约为12小时。因此在地球表面任何地区、任何时候都可以至少同步接收四颗以上的卫星信号。GPS不断向地面发送导航电文，地面上任何位置只需根据四颗卫星信道的电文，就可以算出当前接收者的三维位置，从而实现全球、全天候、三维位置、三维速度的信号接收和实时定位导航。GPS系统拥有许多优点：使用低频信号，纵使天候不佳仍能保持相当的信号穿透性；全球覆盖；三维定速定时精度高；快速、省时、高效率；应用广泛、多功能；可移动定位；不同于双星定位系统，使用过程中接收机不需要发出任何信号，增加了隐蔽性，提高了其军事应用效能。飞行器的传感器标定和测量模型(2)标定为了精密测量GPS卫星C/A信号发射通道时延，可以采用数字信号处理技术进行标定。对GPS卫星输出的C/A导航信号和时间保持系统的Z计数脉冲进行双通道同步采样，在数字域完成C/A信号预处理后，进行多速率数字信号处理，获取C/A信号中伪随机码起始点；结合Z计数脉冲信号样本数据的处理结果，计算得到C/A信号发射通道的绝对时延。采样率为10GHz时算法标定结果的不确定度低于0.2ns。首先，对C/A信号采样数据进行数字下变频，得到中频载波调制的C/A信号。保证抽取后的各子路信号频谱不混叠情况下，选择合适的抽取因子

，对中频载波调制的C/A信号进行抽取，形成

子路信号。设一个伪随机码周期的C/A信号样本个数为

，则抽取后的每个子路数据长度

，任意一个子路i的数据表示为飞行器的传感器标定和测量模型(2)标定对多速率抽取后形成的各路数据分别进行载波-相位二维相关捕获，每个抽取子信号中最大相关峰值点作为该子信号的PRN码起始点。根据抽取率和各子路信号的时延关系，并且减去数字下变频和多速率处理带来的群时延，求取所有子路信号中PRN码起始点对应C/A采样信号中的序号：式中，

为第

子路信号中相关峰值点对应本子路信号样本点的序号；

为由多抽取率数字信号处理带来的时延数；

为数字下变频引起的时延数，等于数字低通滤波器抽头个数的一半。求取最小值，就得到了采样的C/A信号中实际伪码起始点序号：飞行器的传感器标定和测量模型(2)标定对Z计数脉冲信号采样数据进行处理，以Z计数秒脉冲上升沿为GPS卫星C/A信号通道时延基准点，将C/A信号采样数据中实际PRN码起始点的序号与时延基准点序号求差，然后乘以采样周期，减去电缆和衰减器的校准时延，得到C/A信号发射通道绝对时延：式中，

为导航信号绝对时延；

为采样频率；

表示Z计数脉冲上升沿对应序号；

为电缆和衰减器的校准时延。(3)测量信号GPS系统包括三部分:导航卫星、地面站组和用户设备。飞行器的传感器标定和测量模型(13)①导航卫星一共有24颗导航卫星，基本上均匀分布在六个轨道平面内，轨道面相对于赤道平面的倾角为55°，各个轨道平面之间的交角为60°，位于离地面约两万千米的轨道，这些卫星绕地球一周的时间约为12小时，也就是当地球自转360°时，它们绕地球运行约两圈。卫星姿态采用三轴稳定方式，保证了卫星上导航天线辐射口总是对准地面。②地面站组地面站组包括一个主控站、五个监控站和三个注入站。五个监控站都是无人数据收集中心，在主控站控制下跟踪接收24颗卫星发射的取频导航信号，每6s进行一次伪距测量和多普勒观测、采集气象要素等数据，定时将观测数据送到主控站。主控站根据所采集的数据计算每一颗卫星的星历时钟改正数状态参数、大气改正数等,并按一定的格式编辑为导航电文送到注入站。注入站将主控站送来的导航信息注入卫星。此外，注入站还负责监测注入卫星的导航信息是否正确。飞行器的传感器标定和测量模型③用户设备GPS系统采用无源工作方式，凡是有GPS导航接收设备的用户都可以使用该系统。用户设备包括GPS接收机和处理控制解算显示设备。GPS接收机的结构基本分为大天线单元和接收单元两大部分。天线接收卫星发出的导航信号，控制设备进行信号和信息处理，从中得到卫星星历、距离及距离变化率、时钟校正、大气校正参量等，从而算出用户在宇宙直角坐标系中的坐标，同时换算成用户需要的地理坐标等，并在显示器上显示。飞行器的传感器标定和测量模型8.摄像机(1)摄像机摄像机是一种常见的图像传感器，是飞行机器人视觉系统的输入设备，可以将光的二维图像变换成电信号，以便于存储或者传输。当拍摄一个物体时，此物体上反射的光被摄像机镜头收集，使其聚焦在摄像器件的受光面上，再通过摄像器件把光能转变为电能，便可以得到视频信号。原始的光电信号很微弱，需要使用运放电路进行放大，再经过多种电路进行处理和调整，最后得到的标准信号才能送到摄像机中记录下来。摄像机尺寸小、廉价、工作电压低、功耗小、寿命长、性能稳定、荧光屏的图像残留和视觉残留现象小(2)标定摄像机所获取的图像信息是二维的、平面的，要想用来对飞行机器人进行定位和导航，必须要从中获得三维的、立体的视觉信息。机器视觉的研究目的之一就是使计算机具有通过二维图像认知三维环境信息的能力。飞行器的传感器标定和测量模型(2)标定这种能力不仅使机器能感知三维环境信息中物体的几何信息，包它的形状、位置、姿态、运动等，而且能对它们进行描述存储识别与理解。这些从正维空间点到摄像机成像平面中二维图像点之间映射关系参数的确定，是由摄像机成像建立的几何模型所决定的。该几何模型的参数称为摄像机参数，这些参数必须由实验与计算来确定，实验与计算的过程称为摄像机标定。多年以来，有很多学者对摄像机标定问题进行了研究，也提出了为数众多的标定方法。摄像机标定方法根据实时情况的不同，可以分为离线标定和在线标定；根据标定方式的不同，主要可以归纳为三种：传统标定方法、自标定方法、基于主动视觉的标定方法。飞行器的传感器标定和测量模型①传统标定方法所谓传统标定方法是指用一个结构已知、精度很高的标定块作为空间参考物，通过空间点和图像点之间的对应关系来建立摄像机模型参数的约束,然后通过优化算法来求取这些参数。其基本方法是，在一定的摄像机模型下，基于特定的实验条件如形状、尺寸等已知的定标参照物，经过对其图像进行处理，利用一系列数学变换和计算方法,求取摄像机模型的内部参数和外部参数。该方法大致分为基于单帧图像的基本方法和基于多帧已知对应关系的立体视觉方法。传统方法的典型代表有DLT（DirectLinearTransformation）方法、Tsai的方法、Weng的迭代法。传统标定方法的优点在于可以获得较高的精度，但标定过程费时费力，而且在实际应用中的很多情况下无法使用标定块。因此，在实际应用场合要求的精度很高且摄像机的参数很少变化时，传统标定方法应为首选。飞行器的传感器标定和测量模型②自标定方法作为近年来发展起来的另一类摄像机标定技术，摄像机自标定方法与传统的摄像机标定方法的显著不同之处在于：摄像机自标定方法不需要借助于任何外在的特殊标定物或某些三维信息已知的控制点,而是仅仅利用图像对应点的信息，直接通过图像来完成标定任务。正是这种独特的标定思想赋予了摄像机自标定方法巨大的灵活性，同时也使得计算机视觉技术能够面向范围更为广阔的应用。众所周知，在许多实际应用中，由于经常需要改变摄像机的参数，而传统的摄像机标定方法在此类情况下由于需要借助于特殊的标定物而变得不再合适。与其他两种方法相比，自标定方法的不足之处在于鲁棒性较差，这是因为有些摄像机自定标所得到的解既不是唯一的，也不是稳定的。更准确地说，由约束关系所得到的解在一般情况下是多解的；同时，在图像中含有噪声的情况下，解得的值也与实际值有较大的差别。因此，如何在存在噪声的情况下提高解的稳定性，一直是自定标领域的研究人员试图解决的问题。飞行器的传感器标定和测量模型③基于主动视觉的标定方法鉴于传统方法和自标定方法的不足，人们提出了多种不同的基于主动视觉的摄像机标定方法。所谓基于主动视觉的摄像机标定，是指“已知摄像机的某些运动信息”下标定摄像机的方法。与自标定方法相同，这些方法大多是仅利用图像对应点进行标定的方法,而不需要高精度的标定块。“已知摄像机的某些运动信息”包括定量信息和定性信息：定量信息，如摄像机在平台坐标系下朝某一方向平移某一已知量；定性信息，如摄像机仅做平移运动或仅做旋转运动等。基于主动视觉摄像机标定方法的主要优点是，由于提标定过程中已知摄像机的运动信息，所以一般来说，摄像机的模型参数可以线性求解，且计算简单,鲁棒性较高。其缺点是不能适用于摄像机运动未知或无法控制的场合。飞行器的传感器标定和测量模型飞行器的传感器标定和测量模型图4小孔成像原理(3)测量模型摄像机成像模型解决的是将三维场景中的坐标与摄像机得到图像的二位坐标联系起来。常用的摄像机成像模型有三种：小孔成像模型、正交投影模型、拟透视投影模型。其中，最常用的模型是小孔成像模型。如图4的小孔成像模型中，将光心当做小孔，并假设光线满足直线传播的条件。在三维场景中的点，经过光心在像平面上投影成倒像点。显然，小孔成像模型主要由光心，光轴，成像平面三个部分组成。但是，在实际的环境中，由于小孔的透光量非常小，在成像平面形成清晰地像需要较长的时间曝光，所以实际的摄像机的光学系统大多是由透镜组成。飞行器的传感器标定和测量模型(3)测量模型摄像机线性成像模型中坐标的转换有以下三种关系：①从世界坐标系到摄像机坐标系的坐标转化由于摄像机与被摄物体可以放置在环境中任意位置，这样就需要在环境中建立一个坐标系来表示摄像机和被摄物体的位置，这个坐标系就称为世界坐标系。如图5所示，以摄像机光心为原点

，光轴为

轴，建立的直角坐标系即为摄像机坐标系。图5摄像机线性成像模型中坐标的转换关系飞行器的传感器标定和测量模型(3)测量模型摄像机坐标与世界坐标的转换可以通过旋转矩阵

和一个平移向量

完成

，表示为其齐次坐标表示为其中，t为三维平移向量，R为

维的单位正交旋转矩阵，其矩阵元素满足：(14)飞行器的传感器标定和测量模型(3)测量模型②摄像机坐标向图像物理坐标的转换摄像机的线性成像模型如图5所示，三维场景任意一点

在摄像机成像平面上的投影为

，它是摄像机光点

与场景中物点

的连线与摄像机成像平面的交点，这种关系也称为透视投影。图像物理坐标是一个二维坐标，它的原点是摄像机光轴与成像平面的交点，即光点。

轴与

轴平行，

轴与

轴平行，可以得到摄像机坐标投影到图像物理坐标的转换公式：用齐次坐标与矩阵形式表示为(15)飞行器的传感器标定和测量模型(3)测量模型③图像物理坐标向图像像素坐标的转换摄像机拍摄的图像在计算机中以

矩阵的形式进行存储，其中一个单元称之为像素。显然，在图像上定义的图像像素坐标系是一个二直角坐标系，如图6所示。一般情况下，图像像素坐标系的原点取图像的左上角，u轴与图像物理坐标系的x轴平行，v轴与图像物理坐标系y轴平行。在图像像素坐标系中，一点的坐标

表示其行数和列数，其单位为像素。要知道其图像物理坐标必须需要每个像素在u和v方向物理长度k和l。摄像机的光轴与成像平面的交点，即为图像物理坐标的原点，同时也成为图像的主点，主点理论上位于图像的中心，但是由于摄像机制造技术的缺陷，主点往往会偏移图像的中心。图6图像像素坐标与图像物理坐标

飞行器的传感器标定和测量模型(3)测量模型设主点在图像像素坐标系中的坐标的为

就可以得到图像像素坐标与图像物理坐标之间的转换关系如下：其齐次坐标与矩阵形式表示为由(14)、(15)、(16)式可得图像坐标与世界坐标之间的转换关系：

为投影矩阵；

为内参数矩阵，由像素焦距和主点坐标等摄像机内部参数所决定，内参数矩阵在后续的标定过程中将是一个不变的量；

称为外参数矩阵，它由旋转矩阵

和平移向量

决定，它们描述的是摄像机相对于世界坐标系的位置和方向。(16)(17)飞行器的运动状态估计1.姿态估计由于飞行器的体积小、自身有效载荷小，所以一些高精度姿态测量装置无法应用，大多使用由陀螺仪和加速度计组成的低成本惯性测量单元测量姿态。而低成本陀螺仪存在积分累计误差，加速度计在动态姿态测量时易受高频噪声影响，故测量精度较低。于是，姿态信息估计成为了飞行器设计中不可或缺的环节。下面主要以四旋翼无人飞行器为例，介绍几种常用的姿态估计算法。（1）四元数法四旋翼无人飞行器姿态解算是将飞行器上惯性单元的输出实时转换成飞行器的姿态，即飞行器的机体坐标系

相对于导航坐标系

的角位置，它们之间的关系如下式所示：飞行器的运动状态估计（1）四元数法欧拉角是飞行器的3个姿态角，即俯仰角、横滚角、偏航角。根据欧拉旋转定律，可用3次旋转使得机体坐标系与导航坐标系重合，每一次旋转都是以导航坐标系的x、y、y轴中的一个坐标轴来转动，转过的角就是欧拉角，每次旋转后坐标关系可由一旋转矩阵来表示，即方向余弦矩阵：式中

、

、

分别代表偏航角、横滚角、俯仰角。为避免欧拉角在表示姿态时可能出现的奇异问题，四元数在飞行器的姿态表示方面得到了广泛的应用。设描述四旋翼飞行器姿态的四元数为其中，

，

，

，(18)飞行器的运动状态估计（1）四元数法导航坐标系与机体坐标系之间的坐标关系可用方向余弦矩阵表示，其四元数形式为导航坐标系到机体坐标系的旋转过程中坐标系始终保持直角坐标系，所以T为正交矩阵。可得飞行器的姿态角为将四元数代入可得(19)飞行器的运动状态估计（1）四元数法四元数的微分方程为

，即式中

、

、

为机体坐标系下的角速度。在已知初始四元数的情况下，通过三轴陀螺仪测量的3个轴的角速度就可以实时更新四元数的值，进而更新姿态角获得姿态信息。（2）互补滤波补偿算法陀螺仪存在积分误差，所以解算出来的姿态角也会出现偏差。为了解决这一问题，可以引入互补滤波算法，利用加速度计来修正陀螺仪的误差。设加速度计测出来的重力向量为

、

、

，陀螺仪积分后的姿态推算出来的重力向量为

、

、

，则有(20)飞行器的运动状态估计（2）互补滤波补偿算法从而陀螺仪积分后的姿态结合加速度计数据得到的姿态误差为此姿态误差与陀螺仪积分误差成正比，使用互补滤波算法来修正陀螺仪角速度积分误差：其中

、

、

是对陀螺仪的纠正量的积分项，

为积分系数。其中

、

、

为陀螺仪修正后的输出角速度，

为比例系数。(21)飞行器的运动状态估计（2）互补滤波补偿算法在实际测试中，上位机接收到姿态角的数据仍然存在少许的噪声干扰，为了滤波噪声的干扰，引入卡尔曼滤波算法。设姿态角为

，对其离散化并构造状态方程和观测方程：算法步骤如下：①状态一步预测。通过第

时刻的

值，预测第

时刻的

值：

；②一步预测均方误差。根据第

时刻的系统误差估计第

时刻的系统预测误差：

；③滤波增益计算：

；④状态更新，计算系统最有估算值：

；⑤滤波均方误差更新。在卡尔曼滤波运算中要实现对均方误差的更新，以便下一刻计算卡尔曼增益，更新方程如下：

。飞行器的运动状态估计2.位置估计在飞行器控制中，对飞行器在飞行过程中的实时位置估计是相当重要的，不同类型的飞行器飞行位置和轨迹都有区别，对飞行器的运动轨迹进行估计，可判断飞行器的飞行目标与用途。在地球引力、空气阻力和推进剂的作用下，飞行器的运动遵循基本的运动规律，我们称之为运动方程。许多不同的飞行器都遵循相同类型的运动方程。本节将介绍运用变步长四阶五级龙格-库塔算法对已有的含参微分方程进行求解，再对解进行三次样条插值，得到特定时刻下飞行器的位置。下面对变步长四阶五级龙格-库塔算法进行介绍。首先建立基础坐标系，取为随地心平移的坐标系。取地球中心

为原点，地球自转轴为

轴，指向北极为正向，

轴由

指向零时刻的0经度线，再按右手系确定

轴，建立直角坐标系

。根据变质量质点动力学，基础坐标系下的飞行器在主动段下简化运动方程：(22)飞行器的运动状态估计结合飞行器的初始位置与速度，估计出飞行器在任何时刻的位置。其中矢量

指飞行器承受外力加速度之和，

为其位置矢量；

是

对时间的二阶导数；

为地球引力常数，取

。借助MATLAB语言，采用四阶五级龙格-库塔函数求解，并设置第一次求解的相对误差容许上限值。第一次求解结束后，对容许误差上限进行优化，再求解，判定解的可信度之后，对此解进行三次样条差值，提取对应需要时刻飞行器的位置。

四阶龙格-库塔算法是数值分析课程中经常介绍到的传统算法，在求解微分方程中十分有效。德国学者Felhberg改进了传统Runge-Kutta算法，在每个计算步长内对fi(·)函数进行6次求值，称四阶五级Runge-Kutta-Felhberg(RKF)算法，它是定步长方法。在实际应用场景下，根据估计时刻的要求来设定和修正步长值，可以保证计算精度，因此采用变步长算法求解微分方程计算得到的估计值更加精度。在进行数值求解微分方程时，如果操作不当会导致得出不可信结果。所以在微分方程求解后，采用微分方程数值解的验证与三次样条插值对结果进行科学检验。飞行器的运动状态估计将迭代中的相对误差容许上限设置成一个更小的值，观察所得结果和原来的结果是否一致。三次样条插值是通过一系列形值点的一条光滑曲线，通过求解三弯矩方程组得出曲线函数组。用四阶五级Runge-Kutta-Felhberg函数求解微分方程，首次求解时将相对误差容许上限设为，求解完毕再设为，如果两次求解得出的数据点相近，则判定解是可信的。对此可信解进行三次样条差值，即可提取对应时刻飞行器的三维位置。飞行器的运动状态估计3.速度估计除了飞行器的姿态、位置以外，速度也是其重要的参数之一。空速管是最常用的测量飞行器空速的仪表，但对于大马赫数、大迎角等条件的飞行器而言已经不大适用，并且还存在安装、维护困难等问题。随着航天航空技术的飞速发展，大气数据测量系统越来越受重视，产生了许多新的测量方法，如嵌入式大气数据传感系统，相对于传统大气数据测量系统，它在精度、可靠性上都有更大的优势；基于稀疏协方差矩阵迭代的单快拍气流速度估计算法，计算量更低，具有更强的实时性。（1）基于空速管测量空速管也叫皮托管，它是感受气流的总压和静压，并将测得的压力数据传送给大气数据计算机、飞行仪表的装置。它的安装位置一定要在飞行器外面气流较少受到飞行器影响的区域，一般在正前方，垂尾或翼尖前方，同时为了保险起见，通常应该安装两套以上空速管。飞行器的运动