機器人操作系統（ROS）課件7.2Ublox-GPS模塊的使用及坐標轉換

機器人操作系統（ROS）機器人操作系統（ROS）Ublox-GPS模塊的使用及坐標轉換7.2機器人操作系統（ROS）7.2.1GPS簡介

GPS是由一種具有全方位、全天候、全時段、高精度的基于空間的衛(wèi)星導航系統，能在任何時間、任何天氣、任何地點為全球用戶提供低成本、高精度的三維位置、速度和精確定時等導航信息。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(已跟蹤衛(wèi)星數)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接收機輸出的經緯度信息轉換為大地平面坐標來進行導航，常用的坐標轉換方法為高斯-克呂格投影變換。

高斯-克呂格投影示意圖如圖所示，該投影是用一個假想的橢圓柱筒橫置于地球表面，與地球上的某一經線相切，即投影后的中央經線，該橢圓柱的中心軸位于赤道平面內，然后按照一定投影條件將地球橢球面中央子午線兩側規(guī)定范圍內的點投影到橢球圓柱面上，從而得到地球橢球面上各個點的高斯投影。高斯投影示意圖機器人操作系統（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(): #初始化節(jié)點

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融合中心緯度#發(fā)布話題，話題名稱/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]本節(jié)提供經緯度坐標轉換示例代碼getGPS.py。getGPS.py的主要功能為將經緯度坐標轉換為大地平面坐標，訂閱/fix話題（經緯度原始數據），發(fā)布/gps/flat話題（轉換后的大地坐標位置數據）。機器人操作系統（ROS）7.2.2高斯-克呂格投影變換啟動GPS后，運行如下命令進行經緯度轉換：$rosrungps_pubgetGPS.py運行如下命令，查看話題列表，話題/gps/flat內容為經緯度轉換后的坐標，如圖所示：$rostopiclist

查看話題列表機器人操作系統（ROS）7.2.3GPS的使用本節(jié)使用的是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查看端口號通過如下命令修改端口權限為可讀可寫可執(zhí)行，每次拔插Ublox模塊之后需重新設置：$sudochmod777/dev/ttyUSB0機器人操