機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）Ublox-GPS模塊的使用及坐標(biāo)轉(zhuǎn)換7.2機(jī)器人操作系統(tǒng)（ROS）7.2.1GPS簡介

GPS是由一種具有全方位、全天候、全時段、高精度的基于空間的衛(wèi)星導(dǎo)航系統(tǒng)，能在任何時間、任何天氣、任何地點(diǎn)為全球用戶提供低成本、高精度的三維位置、速度和精確定時等導(dǎo)航信息。GPS中最有用的信息是在GGA中，提供了通用的Fix數(shù)據(jù)以及GPS的3D位置。完整的一幀GPS數(shù)據(jù)如圖所示。$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47WhereGGA GlobalpositioningSystemFixData(全球定位修正數(shù)據(jù))123519 Fixtakenat12:35:19UTC(修正)4807.038,N Latitude48deg07.038’N(緯度)01131.

000,E Longitude11deg31.000’E(經(jīng)度)1Fixquality: 0=invalid(修正質(zhì)量)1=GPSfix(SPS)(修正)2=DGPSfix(修正)3=PPSfix(修正)4=RealTimeKinematic(實(shí)時運(yùn)動學(xué))5=FloatRTK6=estimated(deadreckoning)(2.3feature)(估計(jì)航路推算)7=Manualinputmode(手動輸入模式)8=Simulationmode(仿真模式)08 Numberofsatellitiesbeingtracked(已跟蹤衛(wèi)星數(shù))0.9 Horizontaldilutionofposition(水平位置精度)545.4,MAltitude,Meters,abovemeansealevel(高度，米，平均海平面)46.9,M Heightofgeoid(meansealevel)aboveWGS84ellipsoid(大地水準(zhǔn)面高（平均海平面）高于WGS84橢球)(emptyfield)timeinsecondssincelastDGPSupdate(自上一次DGPS更新時間間隔，以秒為單位)(emptyfield)DGPSstationIDnumber(DGPS基站ID號)*47 thechecksumdata,alwaysbeginswith*(校驗(yàn)和數(shù)據(jù))GPGGA格式機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換

地面運(yùn)動機(jī)器人一般采用平面直接坐標(biāo)系，因此需要將GPS接收機(jī)輸出的經(jīng)緯度信息轉(zhuǎn)換為大地平面坐標(biāo)來進(jìn)行導(dǎo)航，常用的坐標(biāo)轉(zhuǎn)換方法為高斯-克呂格投影變換。

高斯-克呂格投影示意圖如圖所示，該投影是用一個假想的橢圓柱筒橫置于地球表面，與地球上的某一經(jīng)線相切，即投影后的中央經(jīng)線，該橢圓柱的中心軸位于赤道平面內(nèi)，然后按照一定投影條件將地球橢球面中央子午線兩側(cè)規(guī)定范圍內(nèi)的點(diǎn)投影到橢球圓柱面上，從而得到地球橢球面上各個點(diǎn)的高斯投影。高斯投影示意圖機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換為了有效控制投影變形，可采用分帶投影的方法。先按一定的經(jīng)度差將地球表面劃分為若干投影帶，再使圓柱面依次和每一帶的中央經(jīng)線相切，并把各帶中央經(jīng)線東西兩側(cè)一定經(jīng)度差范圍內(nèi)的經(jīng)緯線網(wǎng)投影到圓柱上，然后從兩極將該圓柱面切開展平，構(gòu)成地球各帶經(jīng)緯線網(wǎng)在平面上的圖形。6度帶和3度帶的分帶投影示意圖如圖所示，6度分帶從0度經(jīng)線起自西向東每6度分為一個投影帶，將地球劃分為60個投影帶，并依次進(jìn)行編號。6度帶和3度帶的分帶投影機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換defcallback(gps): globalframe_id #gps提供的經(jīng)緯度數(shù)據(jù)轉(zhuǎn)換為以度為單位的經(jīng)緯度格式

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 #經(jīng)緯度轉(zhuǎn)換為大地坐標(biāo)系下的坐標(biāo)

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é)點(diǎn)

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!")機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyimportcv2importnumpyasnpimportmessage_filtersimportmathfromsensor_msgs.msgimportNavSatFixfromgeometry_msgs.msgimportPoseStamped,Pose,Quaternion,Point#定義參數(shù)frame_id=0RefCenterLon=118.79232402#lon融合中心經(jīng)度RefCenterLat=31.93936970#lat融合中心緯度#發(fā)布話題，話題名稱/gps/flat，數(shù)據(jù)類型為Point，隊(duì)列長度為1gps_pub=rospy.Publisher('/gps/flat',Point,queue_size=1)#經(jīng)緯度轉(zhuǎn)換為大地坐標(biāo)系下的坐標(biāo)函數(shù)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#極點(diǎn)處子午線的曲率半徑 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é)提供經(jīng)緯度坐標(biāo)轉(zhuǎn)換示例代碼getGPS.py。getGPS.py的主要功能為將經(jīng)緯度坐標(biāo)轉(zhuǎn)換為大地平面坐標(biāo)，訂閱/fix話題（經(jīng)緯度原始數(shù)據(jù)），發(fā)布/gps/flat話題（轉(zhuǎn)換后的大地坐標(biāo)位置數(shù)據(jù)）。機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換啟動GPS后，運(yùn)行如下命令進(jìn)行經(jīng)緯度轉(zhuǎn)換：$rosrungps_pubgetGPS.py運(yùn)行如下命令，查看話題列表，話題/gps/flat內(nèi)容為經(jīng)緯度轉(zhuǎn)換后的坐標(biāo)，如圖所示：$rostopiclist

查看話題列表機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用本節(jié)使用的是GPS接收模塊Ublox，如圖所示，通過USB轉(zhuǎn)TTL模塊進(jìn)行Ublox和計(jì)算機(jī)的連接，安裝相應(yīng)驅(qū)動，可以從這個設(shè)備中得到相應(yīng)的經(jīng)度、緯度和高層信息。Ublox實(shí)物圖機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用

在開始之前我們需要安裝NMEAGPS的安裝包，安裝完后運(yùn)行rosstack和rospack配置文件，運(yùn)行指令如下：$sudoapt-getinstallros-melodic-nmea-*$rosstackprofile&rospackprofile安裝結(jié)果圖

rosstack和rospack配置結(jié)果圖機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用通過如下命令查看GPS模塊的端口號$ls/dev查看端口號通過如下命令修改端口權(quán)限為可讀可寫可執(zhí)行，每次拔插Ublox模塊之后需重新設(shè)置：$sudochmod777/dev/ttyUSB0機(jī)器人操