基于MIMU的相对定位技术研究+MATLAB仿真(7)
其中， ， ， 分别为公式（3.1）中计算得到的相应时刻的 ， ， 。因此，由传感器测量得到的 ， ， ，即可得到移动机器人每个时刻的位置。
3.3 基于光电编码器的机器人导航系统
3.3.1光电编码器原理
旋转编码器又称码盘，其主要功能是将传动轴旋转角度离散化和量化输出。在移动机器人领域，旋转编码器常被用于测量旋转角度、推算机器人运动速度和位置。旋转编码器根据工作原理分为光电式、磁场式和感应式等，其中最常用的是增量式光电旋转编码器。
增量式光电编码器由光源、屏蔽光的固定光栅、与旋转轴固定的带细光栅的码盘以及光检测器件组成。当码盘旋转时，根据固定和运动光栅的排列，穿透光检测器件的光量变化，离散化和量化后输出脉冲方波。通过计算旋转增量式编码器输出脉冲数目即可解算出载体运动速度、移动距离和方位。
若旋转编码器安装在机器人驱动轮上，编码器每转周期数为N、计数时间为T、脉冲数目n和驱动轮半径为r，则驱动轮运动线速度及路程为
（3.3）
式中，v和s分别为在计数时间T内驱动轮平均运动速度和运动路程。在水平面内，根据机器人运动学方程和编码器输出，机器人方位角度是可以计算得到的。 编码器是一种性价比非常高的传感器，编码器内部结构简单，误差小，在使用中常常被认为是无误差的传感器。由编码器构成机器人定位系统的定位误差源包括机器人驱动轮打滑和解算方位误差。
3.3.2 基于光电编码器导航原理
图3.2机器人示意图
如图3.2所示，取左右两轮中心连线的中点M作为参考点， 和 分别为左右驱动轮的角速度，通过光电编码器测得其转速。
和 分别是机器人前行的线速度和角速度，r和b分别是轮子的半径和两轮的间距，其运动学方程为：
其中 ， 。
和 为传感器的测量值，r与b为与移动机器人有关的固定数值。
和方程（3.1）相似，方程（3.4）得到的 ， ， 分别为世界坐标系中，沿X，Y轴的速度以及偏向角的转速。再由式（3.2）得出移动机器人每一时刻的位置与航向，以上就是基于光电编码器的机器人导航原理。
3.4 基于MIMU和光电编码器的组合导航系统
组合导航系统的基本思想是利用具有互补特性的两个或者两个以上的子系统，借助计算机技术和信息融合技术，构成性能优于其中任何一个子系统的组合系统，通过提取各系统的误差并校正，以满足更高的定位要求。下面首先对卡尔曼滤波的原理及其在组合导航系统中的应用结构进行分析；然后设计针对本课题研究内容的卡尔曼滤波器。
3.4.1卡尔曼滤波原理及其应用结构
3.4.1.1离散卡尔曼滤波算法
卡尔曼滤波(KF)是解决状态最优估计的一种常用方法。采用KF进行最优估计，首先要正确建立系统的状态方程和观测方程。以动态离散随机系统为例，其状态方程和观测方程为：
式中： 为 时刻的n文状态矢量，即被估计的状态量；Z(k)为 时刻的m文观测量； 为状态矢量 从 时刻转移到 时刻的一步转移矩阵，由所选择的系统动力学模型决定； 为 时刻的系统噪声矢量 对 时刻的状态矢量 影响的噪声系数矩阵； 为 时刻的观测矢量 与 时刻的状态矢量 之间的观测系数矩阵，为m×n阶量测矩阵； 为 时刻的系统噪声序列； 为 时刻的m文量测噪声序列。
应用卡尔曼滤波方程组必须满足以下假设条件：
下一篇：基于燃气压力的引信保险机构设计