本文記錄在Ubuntu22.04-Humbel中使用NMEA協(xié)議GPS模塊的過(guò)程，使用國(guó)產(chǎn)ROS開(kāi)發(fā)板魯班貓(LubanCat )進(jìn)行調(diào)試。

一、GPS模塊介紹

在淘寶找了款性價(jià)比較高的輪趣科技GPS北斗雙模定位模塊作為入門(mén)學(xué)習(xí)使用，支持GNSS系統(tǒng)(北斗、GPS、GLONASS、日本的QZSS以及衛(wèi)星增強(qiáng)系統(tǒng)SBAS)，定位精度在2.5m左右，屬于民用級(jí)別。引出了常用的幾個(gè)外部接口，主要用type-c調(diào)試比較方便，售后技術(shù)也相當(dāng)給力。接線示意圖如下，注意：測(cè)試時(shí)需要將GPS天線放置在室外，否則模塊將沒(méi)有GPS信號(hào)

二、坐標(biāo)轉(zhuǎn)換

機(jī)器人基于Ubuntu-ROS平臺(tái)做開(kāi)發(fā)調(diào)試，所以需要將GPS模塊的經(jīng)緯度和高度信息轉(zhuǎn)化為機(jī)器人所能認(rèn)識(shí)的本地坐標(biāo)系(局部笛卡爾坐標(biāo)系)。參考其他博主案例，使用半正矢公式來(lái)將經(jīng)緯度轉(zhuǎn)為xyz坐標(biāo)值。

轉(zhuǎn)換原理

半正矢公式介紹:
半正矢公式是一種根據(jù)兩點(diǎn)的經(jīng)度和緯度來(lái)確定大圓上兩點(diǎn)之間距離的計(jì)算方法，在導(dǎo)航有著重要地位。它是球面三角學(xué)中“半正矢定理”公式的特例，該定理涉及了球面三角形的邊和角。
對(duì)于任何球面上的兩點(diǎn)，圓心角的半正矢值可以通過(guò)如下公式計(jì)算：

??????????hav($\frac{d}{r}$)=hav(${\psi }_2$-${\psi }_1$)+cos(${\psi }_1$)cos(${\psi }_2$)hav(${\lambda }_2$-${\lambda }_1$)

hav 是半正矢函數(shù)的縮寫(xiě)：

??????????haversin($\theta$)=$si{n}^2$($\frac{(\theta )}{2}$)=$\frac{1?cos(\theta )}{2}$

參數(shù)解釋：

d ：兩點(diǎn)之間的距離（沿大圓，見(jiàn)球面距離）；
r ：球的半徑；
${\psi }_1$${\psi }_2$ ：點(diǎn) 1 的緯度和點(diǎn) 2 的緯度，以弧度制度量；
${\lambda }_1$${\lambda }_2$：點(diǎn) 1 的經(jīng)度和點(diǎn) 2 的經(jīng)度，以弧度制度量。
等號(hào)左邊的$\frac{d}{r}$：圓心角，單位是弧度。

所以，可以通過(guò)應(yīng)用反半正矢函數(shù)（如果可以查到值）或通過(guò)使用反正弦函數(shù)來(lái)解出d ：
??????????$$d=\sqrt[]{r?archav(h)}$$??????????????????=??2r*arcsin$\sqrt[]{h}$)

將h=hav$\frac{d}{r}$代入后可得：
???
對(duì)于中短距離來(lái)說(shuō)，半正矢公式是計(jì)算地球表面 GPS 坐標(biāo)之間距離的有用且相對(duì)準(zhǔn)確的方法，在距離較長(zhǎng)時(shí)其準(zhǔn)確性可能會(huì)降低。

增加回調(diào)函數(shù)

這里簡(jiǎn)單解釋一下gpsCallback回調(diào)函數(shù)的內(nèi)容，此處訂閱sensor_msgs::msg::NavSatFix數(shù)據(jù)格式的GPS話題進(jìn)行數(shù)據(jù)處理。

//將緯度和經(jīng)度值從度數(shù)轉(zhuǎn)換為弧度。
double rad(double d) 
{
	return d * 3.1415926 / 180.0;
}
void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr gps_msg)
{
    if (!pose_init)
    {
        // Initialization code remains the same.
        //使用接收到的 GPS 消息中的緯度、經(jīng)度和高度f(wàn)alse進(jìn)行初始化，只初始化一次
        init_pose.latitude = gps_msg->latitude;
        init_pose.longitude = gps_msg->longitude;
        init_pose.altitude = gps_msg->altitude;
        init = true;
    }
    else
    {
        //計(jì)算當(dāng)前 GPS 位置與初始 GPS 位置之間的緯度和經(jīng)度變化
        double radLat1 ,radLat2, radLong1,radLong2,delta_lat,delta_long,x,y;
        double deltaLat_y ,deltaLong_x;
        deltaLat_x = 0; 
        deltaLong_y = 0;
        radLat1 = rad(init_pose.latitude);//初始化位置的緯度
        radLong1 = rad(init_pose.longitude);//初始化位置的經(jīng)度
        radLat2 = rad(gps_msg->latitude);//當(dāng)前位置的緯度
        radLong2 = rad(gps_msg->longitude);//當(dāng)前位置的經(jīng)度
        deltaLat = radLat2 - radLat1; 
        deltaLong = radLong2 - radLong1;
 
        // 代入公式進(jìn)行計(jì)算x的坐標(biāo)值
        x = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(deltaLong_y / 2), 2));
        
        //公式計(jì)算結(jié)果默認(rèn)為弧度，所以這里要轉(zhuǎn)換成m的單位
        x = x * EARTH_RADIUS * 1000; 

        //  代入公式進(jìn)行計(jì)算y的坐標(biāo)值
        y = 2 * asin(sqrt(pow(sin(deltaLat_x / 2), 2) + cos(radLat2) * cos(radLat2) * pow(sin(deltaLong / 2), 2));
        y = y * EARTH_RADIUS * 1000; // Convert to meters.

        // 將高度差計(jì)算為z的坐標(biāo)值
        double z = gps_msg->altitude - init_pose.altitude;

        //將計(jì)算得到的xyz值發(fā)布為ROS中的PoseStamped數(shù)據(jù)，作為路徑發(fā)布
        
        ros_path_.header.frame_id = "path";
        ros_path_.header.stamp = rclcpp::Node::now();
        geometry_msgs::msg::PoseStamped pose;
        pose.header = ros_path_.header;
        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.position.z = z;
        ros_path_.poses.push_back(pose);
        state_pub_.publish(ros_path_);
    }
}

效果演示

使用nmea_navsat_driver驅(qū)動(dòng)啟動(dòng)GPS模塊，源碼鏈接：https://github.com/ros-drivers/nmea_navsat_driver/tree/ros2

ros2 launch nmea_navsat_driver nmea_serial_driver.launch.py

然后啟動(dòng)坐標(biāo)轉(zhuǎn)換節(jié)點(diǎn)，將經(jīng)緯度數(shù)據(jù)從WGS-84 坐標(biāo)轉(zhuǎn)換到機(jī)器人真實(shí)世
界 xyz 坐標(biāo)系下（一般為東北天方向），以第一個(gè)經(jīng)緯度數(shù)據(jù)為原點(diǎn)，不斷描繪實(shí)時(shí)經(jīng)緯度數(shù)據(jù)的位移方向，顯示為機(jī)器人運(yùn)行軌跡圖。

ros2 launch gps_path gps_path.launch.py

打開(kāi)rviz2，修改 Fixed Frame 為path，電機(jī)左下角[add]，根據(jù)topic添加[gps_path]->path后，顯示效果為：
文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-771281.html

到了這里，關(guān)于GPS學(xué)習(xí)（一）：在ROS2中將GPS經(jīng)緯度數(shù)據(jù)轉(zhuǎn)換為機(jī)器人ENU坐標(biāo)系，在RVIZ中顯示坐標(biāo)軌跡的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！