机器人操作系统（ROS）课件7.2Ublox-GPS模块的使用及坐标转换

机器人操作系统（ROS）机器人操作系统（ROS）Ublox-GPS模块的使用及坐标转换7.2机器人操作系统（ROS）7.2.1GPS简介

GPS是由一种具有全方位、全天候、全时段、高精度的基于空间的卫星导航系统，能在任何时间、任何天气、任何地点为全球用户提供低成本、高精度的三维位置、速度和精确定时等导航信息。GPS中最有用的信息是在GGA中，提供了通用的Fix数据以及GPS的3D位置。完整的一帧GPS数据如图所示。$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47WhereGGA GlobalpositioningSystemFixData(全球定位修正数据)123519 Fixtakenat12:35:19UTC(修正)4807.038,N Latitude48deg07.038’N(纬度)01131.

000,E Longitude11deg31.000’E(经度)1Fixquality: 0=invalid(修正质量)1=GPSfix(SPS)(修正)2=DGPSfix(修正)3=PPSfix(修正)4=RealTimeKinematic(实时运动学)5=FloatRTK6=estimated(deadreckoning)(2.3feature)(估计航路推算)7=Manualinputmode(手动输入模式)8=Simulationmode(仿真模式)08 Numberofsatellitiesbeingtracked(已跟踪卫星数)0.9 Horizontaldilutionofposition(水平位置精度)545.4,MAltitude,Meters,abovemeansealevel(高度，米，平均海平面)46.9,M Heightofgeoid(meansealevel)aboveWGS84ellipsoid(大地水准面高（平均海平面）高于WGS84椭球)(emptyfield)timeinsecondssincelastDGPSupdate(自上一次DGPS更新时间间隔，以秒为单位)(emptyfield)DGPSstationIDnumber(DGPS基站ID号)*47 thechecksumdata,alwaysbeginswith*(校验和数据)GPGGA格式机器人操作系统（ROS）7.2.2高斯-克吕格投影变换

地面运动机器人一般采用平面直接坐标系，因此需要将GPS接收机输出的经纬度信息转换为大地平面坐标来进行导航，常用的坐标转换方法为高斯-克吕格投影变换。

高斯-克吕格投影示意图如图所示，该投影是用一个假想的椭圆柱筒横置于地球表面，与地球上的某一经线相切，即投影后的中央经线，该椭圆柱的中心轴位于赤道平面内，然后按照一定投影条件将地球椭球面中央子午线两侧规定范围内的点投影到椭球圆柱面上，从而得到地球椭球面上各个点的高斯投影。高斯投影示意图机器人操作系统（ROS）7.2.2高斯-克吕格投影变换为了有效控制投影变形，可采用分带投影的方法。先按一定的经度差将地球表面划分为若干投影带，再使圆柱面依次和每一带的中央经线相切，并把各带中央经线东西两侧一定经度差范围内的经纬线网投影到圆柱上，然后从两极将该圆柱面切开展平，构成地球各带经纬线网在平面上的图形。6度带和3度带的分带投影示意图如图所示，6度分带从0度经线起自西向东每6度分为一个投影带，将地球划分为60个投影带，并依次进行编号。6度带和3度带的分带投影机器人操作系统（ROS）7.2.2高斯-克吕格投影变换defcallback(gps): globalframe_id #gps提供的经纬度数据转换为以度为单位的经纬度格式

gps_jingdu_1=float(gps.longitude)/100 gps_weidu_1=float(gps.latitude)/100 gps_jingdu_2=((gps_jingdu_1-math.floor(gps_jingdu_1))*100)/60 gps_weidu_2=((gps_weidu_1-math.floor(gps_weidu_1))*100)/60 gps_jingdu=math.floor(gps_jingdu_1)+gps_jingdu_2 gps_weidu=math.floor(gps_weidu_1)+gps_weidu_2 #经纬度转换为大地坐标系下的坐标

gps_x=LatLonToXY(gps_weidu,gps_jingdu)[1] gps_y=LatLonToXY(gps_weidu,gps_jingdu)[0] gps_msg=Point() gps_msg.y=gps_x gps_msg.x=gps_y gps_pub.publish(gps_msg)defgps_convert(): #初始化节点

rospy.init_node('get_GPS',anonymous=True) #订阅话题/fix gps_sub=message_filters.Subscriber('/fix',NavSatFix) ts=message_filters.ApproximateTimeSynchronizer([gps_sub],10,0.1) ts.registerCallback(callback) rospy.loginfo("gpsdataconvertfunctionsuccessfullyinitialized!") rospy.spin()if__name__=='__main__’: listener() rospy.loginfo("datasuccessfullysaved!")机器人操作系统（ROS）7.2.2高斯-克吕格投影变换#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyimportcv2importnumpyasnpimportmessage_filtersimportmathfromsensor_msgs.msgimportNavSatFixfromgeometry_msgs.msgimportPoseStamped,Pose,Quaternion,Point#定义参数frame_id=0RefCenterLon=118.79232402#lon融合中心经度RefCenterLat=31.93936970#lat融合中心纬度#发布话题，话题名称/gps/flat，数据类型为Point，队列长度为1gps_pub=rospy.Publisher('/gps/flat',Point,queue_size=1)#经纬度转换为大地坐标系下的坐标函数defLatLonToXY(Lat,Lon): fi=Lat/180*math.pi la=(Lon-RefCenterLon)/180*math.pi zi=RefCenterLat/180*math.pi a=6378137#长半轴 b=6356752.3142#短半轴 c=6399593.6258#极点处子午线的曲率半径 f=1/298.257223563#椭圆率 E2=0.00669437999013#第一偏心率的平方 Eta2=0.00673949674227#第二偏心率的平方 V=math.sqrt(1+Eta2) N=c/V beta0=1.0-3.0/4.0*E2+45.0/64.0*E2**2.0-175.0/256.0*E2**3+11025.0/16384.0*E2**4 beta2=beta0-1 beta4=15.0/32.0*E2**2-175.0/384.0*E2**3+3675.0/8192.0*E2**4 beta6=-35.0/96.0*E2**3+735.0/2048.0*E2**4 beta8=315.0/1024.0*E2**4 Sz=c*(beta0*zi+(beta2*math.cos(zi)+beta4*math.cos(zi)**3+beta6*math.cos(zi)**5+beta8*math.cos(zi)**7)*math.sin(zi)) S=c*(beta0*fi+(beta2*math.cos(fi)+beta4*math.cos(fi)**3+beta6*math.cos(fi)**5+beta8*math.cos(fi)**7)*math.sin(fi)) X=S+la**2*N/2.0*math.sin(fi)*math.cos(fi)+la**4*N/24.0*math.sin(fi)*math.cos(fi)**3.0*(5.0-math.tan(fi)**2+9.0*Eta2+4*Eta2**2)+la**6*N/720.0*math.sin(fi)*math.cos(fi)**5*(61-58*math.tan(fi)**2+math.tan(fi)**4) Y=la*N*math.cos(fi)+la**3*N/6.0*math.cos(fi)**3*(1-math.tan(fi)**2+Eta2)+la**5*N/120.0*math.cos(fi)**5*(5-18*math.tan(fi)**2+math.tan(fi)**4) Z=Sz+la**2*N/2.0*math.sin(zi)*math.cos(zi)+la**4*N/24.0*math.sin(zi)*math.cos(zi)**3.0*(5.0-math.tan(zi)**2+9.0*Eta2+4*Eta2**2)+la**6*N/720.0*math.sin(zi)*math.cos(zi)**5*(61-58*math.tan(zi)**2+math.tan(zi)**4) X=X-Z return[Y,X]本节提供经纬度坐标转换示例代码getGPS.py。getGPS.py的主要功能为将经纬度坐标转换为大地平面坐标，订阅/fix话题（经纬度原始数据），发布/gps/flat话题（转换后的大地坐标位置数据）。机器人操作系统（ROS）7.2.2高斯-克吕格投影变换启动GPS后，运行如下命令进行经纬度转换：$rosrungps_pubgetGPS.py运行如下命令，查看话题列表，话题/gps/flat内容为经纬度转换后的坐标，如图所示：$rostopiclist

查看话题列表机器人操作系统（ROS）7.2.3GPS的使用本节使用的是GPS接收模块Ublox，如图所示，通过USB转TTL模块进行Ublox和计算机的连接，安装相应驱动，可以从这个设备中得到相应的经度、纬度和高层信息。Ublox实物图机器人操作系统（ROS）7.2.3GPS的使用

在开始之前我们需要安装NMEAGPS的安装包，安装完后运行rosstack和rospack配置文件，运行指令如下：$sudoapt-getinstallros-melodic-nmea-*$rosstackprofile&rospackprofile安装结果图

rosstack和rospack配置结果图机器人操作系统（ROS）7.2.3GPS的使用通过如下命令查看GPS模块的端口号$ls/dev查看端口号通过如下命令修改端口权限为可读可写可执行，每次拔插Ublox模块之后需重新设置：$sudochmod777/dev/ttyUSB0机器人操