基于高斯投影的惯导_GPS机器人组合定位研究_赵毅.pdf

电气与自动化赵毅，等基于高斯投影的惯导/GPS 机器人组合定位研究基金项目:国家自然科学基金项目(61801122)第一作者简介:赵毅(1996)，男，安徽六安人，硕士研究生，研究方向为机器人设计与研究、机电系统一体化。DOI:1019344/j cnki issn16715276202301043基于高斯投影的惯导/GPS 机器人组合定位研究赵毅，聂晓根，方少东，吴浩鑫(福州大学 机械工程及自动化学院，福建 福州 350108)摘要:针对移动机器人室外环境下的定位问题，提出一种惯导/GPS 的组合定位算法。通过高斯克吕格坐标投影，将机器人的地理位置坐标转换为直角坐标系坐标，利用 EKF 滤波融合传感器信息进行组合定位，基于 STM32 开发平台搭建移动机器人硬件平台进行实验，并用MATLAB 软件仿真对比，实验和仿真结果表明:该组合定位算法具有较高的定位精度。关键词:移动机器人;高斯投影;EKF 滤波;组合定位中图分类号:TP242文献标志码:A文章编号:1671-5276(2023)01-0173-04esearch on Inertial Navigation/GPS obot Combined PositioningBased on Gaussian ProjectionZHAO Yi，NIE Xiaogen，FANG Shaodong，WU Haoxin(School of Mechanical Engineering and Automation，Fuzhou University，Fuzhou 350108，China)Abstract:To cope with the positioning problem of mobile robots in outdoor environments，a combined inertial navigation/GPS positioningalgorithm is proposed Through Gauss Krger coordinate projection，the robots geographic location coordinates are converted torectangular coordinate system coordinates，EKF filter is used to fuse sensor information for combined positioning，and based on the STM32development platform，the mobile robot hardware platform is built for experiments，with which simulation comparison is performed byMATLAB with the results showing that the proposed combined positioning algorithm has high positioning accuracyKeywords:mobile robot;Gaussian projection;EKF filter;combined positioning0引言随着移动机器人的迅猛发展，对定位导航系统的性能有着更高的要求1，近年来，随着智能领域和机器人技术的快速发展，室外环境下自主移动机器人的定位技术已经成为机器人研究的关键。定位技术不仅可单独用于移动平台的定位，还实现了与其他传感器技术结合2。单一的定位导航系统已经难以满足需求，研究基于多传感器信息融合的组合导航系统已成为新的趋势，如与 GPS 技术结合，运用航位推算误差修正来提高移动机器人定位精度3。定位导航技术是移动机器人控制系统设计中一个十分重要的关键技术，也是机器人寻边建图以及路径规划的前提4。机器人的自主定位是基于上一时刻的姿态估计，利用当前时刻的姿态估计来对移动机器人在环境地图中进行定位5。移动机器人的定位方法主要有以下 3 种:第 1 种是绝对定位，采用导航信标、主动或被动标识、全球定位系统进行定位;第 2 种是相对定位，又称航迹推算，是在已知移动机器人当前时刻位置的条件下，利用里程计、陀螺仪等内部传感器，通过测量机器人相对于初始状态的变化量来定位6;第 3 种是组合定位，这种方法针对前面两种方法的不足，采用算法将相对和绝对定位信息融合在一起7。综上所述，本文提出了一种惯导/GPS 组合自定位算法，为室外移动机器人提供精确稳定的定位信息。首先通过高斯克吕格坐标投影算法，将机器人的地理位置坐标转换为直角坐标系坐标，然后利用 EKF 滤波融合传感器信息，进行组合定位，最后利用 STM32 嵌入式开发平台，搭建移动机器人硬件平台进行实验，并利用 MATLAB 对EKF 滤波、航位推算两种定位方法进行仿真对比。1基于高斯克吕格投影的坐标转换高斯克吕格投影又名“等角横切椭圆柱投影”，是地球椭球面和平面间正形投影的一种8。高斯克吕格投影的本质是等角投影，其目的是将地球的经纬度坐标转换为平面的二维直角坐标，其几何示意图如图 1 所示。图 1高斯克吕格投影示意图371电气与自动化赵毅，等基于高斯投影的惯导/GPS 机器人组合定位研究由于高斯克吕格投影后除中央经线外其他元素的长度都会出现或多或少的改变，这个变化量是随着所在元素与中央经线的距离增大而增大的。因此，需要按照一定的距离给地球表面划分投影带来限制元素长度的变形。本文采用的投影划分形式为 3带，即从东经 15子午线开始，每隔 3经差向东划分为一带，从而实现分带投影，带号依次为 1，2，3，120。因此在已知某点的地理位置为 g(L，W)的条件下(L 为该点的经度，W 为该点的纬度)，可以得到该点的 3带投影带带号公式为n=L153(1)式中 n 为投影带带号。还可以通过投影带带号算出该投影带的中央经度，对于东半球而言，投影带中央经度L0为L0=3n(2)高斯克吕格投影的目的是把地理位置坐标(L，W)转换为机器人工作环境直角坐标系坐标(x，y)。须注意这里的 x 坐标是与中央经线平行的纵向坐标，y 坐标才是横向坐标。在给出高斯克吕格投影公式之前，先对几个地球几何参数做出说明:假设地球为旋转椭球体，且 a 为地球椭球体长半轴的长度，b 为地球椭球体短半轴的长度，e1为地球椭球体的第一偏心率，e2为地球椭球体的第二偏心率，有:e1=a2b2a2(3)e2=a2b2b2(4)除此之外，还需要知道地球的卯酉圈曲率半径N和子午圈曲率半径M，其计算公式为式(5)和式(6)，从赤道到所在点纬度 W 的子午线长度 S，计算公式为式(7)。N=a2/b1+e22cos2W(5)M=a2(1e21)(1+e22sin2W)3/2(6)S=a(1e21)AWB2sin2W+C4sin4WD6sin6W+E8sin8W(7)式(7)中 A、B、C、D、E 各个系数的计算公式如式(8)所示。A=1+34e21+4564e41+175256e61+11 02516 384e81+B=34e21+1516e41+525512e61+2 2052 048e81+C=1564e41+105256e61+2 2054 096e81+D=35512e61+3152 048e81+E=31516 384e81+(8)由以上已知的量，可得高斯克吕格正向投影 x、y 坐标公式为x=S+N2l2sinWcosW+N24l4sinW cos3W(5tan2W+92+44)+N72l6sinW cos5W(6158tan2W+tan4W+27043302tan2W)+y=NlcosW+N6l3cos3W(1tan2W+2)+N120l5cos5W(5+142582tan2W18tan2W+tan4W)+(9)上式中的 l 是该点位置到投影带中央经线位置的经差，即:l=LL0(10)为辅助参数，为=e2cosW(11)2基于 EKF 滤波算法的组合定位模型以 X(k)=x(k)，y(k)，(k)，v(k)，(k)T描述移动机器人 k 时刻的运动状态量，其中 x(k)、y(k)为机器人的坐标矢量;v(k)、(k)为机器人的速度和角速度矢量，当更新时间 T 足够短时，由此可建立移动机器人的状态更新方程为X(k)=f(X(k1)+(k1)w(k1)(12)(k1)为系统过程噪声向量系数矩阵，在这里为(k1)=1000000001000010000100001 (13)由于量测模型是线性模型，则可以建立量测更新状态方程为Z(k)=H(k)Xm(k)+v(k)(14)在以上的量测更新状态方程(14)中，可知 Xm(k)为该系统的状态变量的观测值向量xGPS(k)，yGPS(k)，IMU(k)，vIMU(k)，IMU(k)T，其中xGPS(k)和yGPS(k)是通过GPS 模块获取的经纬度值经高斯克吕格投影和坐标转换后得到的机器人环境坐标系下的 x、y 坐标值，IMU(k)是IMU 模块获取的移动机器人航向角周期变化量，而通过对IMU 模块实时输出的三轴加速度以及三轴角加速度进行积分就能得到移动机器人的速度值vIMU(k)以及角速度值IMU(k)。H(k)则是观测值转换矩阵，为H(k)=1000000001000010000100001 (15)对雅克比矩阵 k/k1的计算如下:k/k1=J(f(X(k1)=f(X(k1)X(k1)T(16)471电气与自动化赵毅，等基于高斯投影的惯导/GPS 机器人组合定位研究通过上面的计算，运用到 EKF 滤波算法中去，对 GPS与 IMU 输出的信息进行融合，再经过一个滤波周期后得到下一个状态估计量，令 k=k+1 重复滤波，从而得到动态的导航定位参数，实现移动机器人的组合定位，其组合定位 EKF 滤波器框图如图 2 所示。图 2EKF 滤波器框图3实验及仿真结果分析31实验平台搭建图 3 为本实验所采用的移动机器人平台，主体由 4 个麦克纳姆轮和车体及硬件系统组成，平台硬件系统包括微处理器模块 STM32F103 驱动板;GPS 模块、IMU 模块、无线通信模块，执行电机为直流无刷电机，以 STM32F103 为主控单元。其中 IMU 模块内部集成高精度的陀螺仪、加速度计、磁力计、气压计，可以输出 10 个轴信息，模块内部的 MCU 采用姿态解算算法以及数字滤波技术对输入的姿态信息进行处理，直接给外部连接的主控芯片输出高精度的数据。图 3移动机器人实验平台32EKF 滤波仿真结果及分析扩展卡尔曼滤波要求动态系统的激励噪声序列 Wk以及量测噪声序列 Vk均为高斯白噪声。根据所选模块的说明书以及实际情况，可知系统的激励噪声序列方差阵 Qk以及量测噪声序列 k分别为Q(k)=100000100000001000000000 1000000001 6 (17)(k)=6250000062500000001000 00000000 4000000002 5 (18)当移动机器人作直线运动时，设定 EKF 滤波仿真的初始条件 X(0)=0，0，45，1，0T，P(0)=zeros(5)，滤波周期 T=05s，总时长 C=1 000s。如图 4 所示，从上到下分别为传感器航位推算轨迹、EKF 滤波定位轨迹、理想轨迹。图 4直线运动轨迹比较图当移动机器人作曲线运动时，设定 EKF 滤波仿真的初始条件 X(0)=0，0，0，1，5T，P(0)=Zeros(5)，滤波周期 T=05 s，总时长为 C=500 s。如图 5 所示，从上到下分别为传感器航位推算轨迹、EKF 滤波定位轨迹、理想轨迹。图 5曲线运动轨迹比较图从图 4图 5 可以看出，当移动机器人作直线和曲线运动时，直接由传感器得出的航位推算进行定位时，其轨迹与理想轨迹偏差较大，且随着时间的推移，其累计下来的误差会逐渐增大。相比而言，采用 EKF 滤波定位，能够更加接近于理想情况。33组合定位结果及分析为了验证本文方法在实际工作环境下