【传感器】GNSS卫星定位导航原理和实现（附代码）

网络营销 2022-12-12 20:04 7199人围观定位

一、相关常识

1.NMEA

NMEA是National Marine Electronics Association 的缩写，是美国国家陆地电子协会的简称，现在是GPS导航装备同一的标准协议。

2.UTM坐标系

Universal Transverse Mercator Grid System，通用横墨卡托网格系统是一种平面直角坐标，这类坐标格网系统及其所根据的投影已经普遍用于地形图，作为卫星影象和自然资本数据库的参考格网以及要求切确定位的其他利用。

例：12U 358657mE 5885532mN

12代表经度12区
U代表纬度U区
358657mE代表东向位置358657m
5885532mN代表北向位置5885532m

二、代码实现

本文代码首要依靠于ROS相关库实现，源代码见：https://github.com/Abin1258/gps_to_odom.git

1.输入：GPS数据

几种常见的数据格式：

1）接收机串口输出：GPFPD等

2）ROS消息格式：nmea_msgs::Sentence ：带消息头的gps裸数据

std_msgs/Header header
uint32 seq
time stamp
string frame_id
string sentence

3）ROS消息格式：sensor_msgs::NavSatFix ：剖析出来的gps范例传感器数据格式

uint8 COVARIANCE_TYPE_UNKNOWN=0
uint8 COVARIANCE_TYPE_APPROXIMATED=1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
uint8 COVARIANCE_TYPE_KNOWN=3
std_msgs/Header header
uint32 seq
time stamp
string frame_id
sensor_msgs/NavSatStatus status
int8 STATUS_NO_FIX=-1
int8 STATUS_FIX=0
int8 STATUS_SBAS_FIX=1
int8 STATUS_GBAS_FIX=2
uint16 SERVICE_GPS=1
uint16 SERVICE_GLONASS=2
uint16 SERVICE_COMPASS=4
uint16 SERVICE_GALILEO=8
int8 status
uint16 service
float64 latitude
float64 longitude
float64 altitude
float64[9] position_covariance
uint8 position_covariance_type

4）ROS消息格式：geographic_msgs::GeoPointStamped ：带消息头的三维点的数据格式

std_msgs/Header header
uint32 seq
time stamp
string frame_id
geographic_msgs/GeoPoint position
float64 latitude
float64 longitude
float64 altitude

本文彩用的是经过GPS驱动剖析后的数据，已经将原始数据封装为rostopic /gps/fix，GPS驱动可以见本链接：https://github.com/Abin1258/gps_gpfpd_drive.git

所以接管GPS数据的具体代码采用的sensor_msgs::NavSatFix数据格式，代码以下：

void GpsOdom::NavsatCallback(const sensor_msgs::NavSatFixConstPtr& navsat_msg) {
geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
gps_msg->header = navsat_msg->header;
gps_msg->position.latitude = navsat_msg->latitude;
gps_msg->position.longitude = navsat_msg->longitude;
gps_msg->position.altitude = navsat_msg->altitude;
GpsCallback(gps_msg);
}

2.输出：里程计Odomerty

在ROS消息中的格式为：

std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance

3.处置进程

由于GPS间接输出的就是经纬度和高度，对应与位置中的XYZ，所以间接将GPS数据转化为UTM坐标系下并已第一帧数据作为起点即可，代码以下：

gps_msg->header.stamp += ros::Duration(0.0);
geographic_msgs::GeoPoint gpspoint;
gpspoint.altitude = gps_msg->position.altitude;
gpspoint.latitude = gps_msg->position.latitude;
gpspoint.longitude = gps_msg->position.longitude;
geodesy::UTMPoint utm;
geodesy::fromMsg(gpspoint, utm);
Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude);

if(!zero_utm) {
zero_utm = xyz;
}
xyz -= (*zero_utm);

odom.header.seq = gps_msg->header.seq;
odom.header.stamp = gps_msg->header.stamp;
odom.pose.pose.position.x = xyz(0);
odom.pose.pose.position.y = xyz(1);
odom.pose.pose.position.z = xyz(2);

odompub.publish(odom);