为物联网应用计算真北
4.97/5 (51投票s)
使用树莓派的 GPS 和指南针找到真北。
引言
在机器人和嵌入式系统中，确定您的地理位置和方向非常有用。使用 GPS 和指南针传感器可以轻松确定您的位置以及相对于磁北的方向。但如果您想知道相对于真北的方向呢？
确定与真北的磁偏角并不难，这也被称为磁偏角。您可以在网上通过 NOAA 网站轻松找到磁偏角。但我想要一个可以让我通过代码在嵌入式设备中使用的实时模型，这就是世界磁场模型 (WMM)！在 Linux 中使用 GPS 和 WMM 可以轻松确定磁偏角，然后您可以将其用于偏移指南针的磁北方向。我将带您一步步了解如何确定磁偏角并使用它来调整您的指南针方向。
背景
我是一名乐高爱好者，也是乐高原型挑战赛的获胜者。我一直在用乐高和树莓派组件制作一个寻星相机机器人。为了确定恒星坐标，我首先需要知道我的机器人相对于真北的地理坐标以及它的朝向，并具有合理的精度。设置 GPS 和指南针相当简单，安装和访问世界磁场模型也相对容易。
这是我第一篇 CodeProject 文章，我的机器人仍在开发中，但已经走了很长一段路。我想与社区分享我已完成的一些工作。希望您喜欢！
您可以在以下链接中找到更多关于我的机器人和其他乐高设备的图片。
星际机器人 (乐高相机)
乐高数码相框
点射红外相机
乐高电脑
使用的组件
您需要设置以下设备，设置说明包含在下面的链接中。我不会详细介绍这些内容，因此您需要对树莓派设置以及 Linux 中的数字和 I2C 传感器有所了解。
1 Adafruit Ultimate GPS Breakout (设置说明)
1 HMC5883L 三轴磁力计 (设置说明)
接线图
所需的库
您需要在树莓派上安装几个库，这些库将允许您通过代码访问 GPS 和磁场模型。指南针不需要额外的库，但它使用 i2c-dev.h。
GPSd (设置说明) (通过 libgpsmm.h 访问 GPS 数据)
GeographicLib (设置说明) (访问 WMM 数据和其他地理相关算法)
读取 GPS 坐标
第一步是从设备读取 GPS 坐标。使用 GPSd 和 libgpsmm.h 可以非常轻松地访问您的 GPS 数据。我们将创建一个类来管理此接口并为我们提供设备流式传输的值。GPS 使用串行连接，libgps 会为您解析返回的句子。我们可以使用返回的原始坐标值，也可以将其分解为度、分、秒等组件。
GPS 类
我创建了一个简单的包装类来管理与 GPS 库的交互，并进行原始值和组件值之间的坐标转换。
#ifndef __GPS_H_INCLUDED__
#define __GPS_H_INCLUDED__
#include "libgpsmm.h" // GPS
class gps
{
private:
// Main entry into GPSd.
gpsmm *gps_rec;
// Private Helper functions for Converting Values.
int degrees(float coordinate);
int minutes(float coordinate);
float seconds(float coordinate);
public:
// Values for our Application to Consume.
int gps_fix;
float gps_lon;
float gps_lat;
float gps_alt;
int gps_sat;
int gps_use;
// Routines for managing interactions with GPSd.
bool start(); // Starts GPSd interface
bool update(); // Retrieves Coordinates from GPSd.
bool stop(); // Shuts down GPSd interface and releases resources.
// Conversion functions to return latitude components from raw GPS values.
int latitude_degrees();
int latitude_minutes();
float latitude_seconds();
// Conversion functions to return longitude components from raw GPS values.
int longitude_degrees();
int longitude_minutes();
float longitude_seconds();
};
#endif
初始化 GPS 流
现在您的类已设置好，您需要初始化您的 GPS 流。这只需要几行代码，您可以在此处修改一些选项，如 GPS 设备地址和流格式，但我使用的是默认设置。您可以在 GPSd 网站上进行其他研究，以确定如何使用这些参数。
bool gps::start()
{
/* Initialize GPS */
gps_rec = new gpsmm("localhost", DEFAULT_GPSD_PORT);
/* Setup GPS Stream */
if (gps_rec->stream(WATCH_ENABLE | WATCH_JSON) == NULL)
{
return false;
}
return true;
}
读取 GPS 流
现在您的设备已设置好，您可以从流中读取值。这也很简单，我创建了一个名为 `update()` 的函数，该函数在主控制循环的每一帧调用一次。我们将使用 libgpsmm.h 提供的 `gps_data_t` 数据结构来存储原始值，然后将其存储在我们的类变量中。`gps_data_t` 返回许多值，您可能需要参考 在线文档 来查看是否有其他值可能对您有用。
bool gps::update()
{
/* Raw GPS Values structure */
struct gps_data_t* gps_data;
/* Wait until device is ready */
if (gps_rec->waiting(00050000))
{
/* See if we're reading data. */
if ((gps_data = gps_rec->read()) == NULL)
{
return false;
}
/* Check for a 2D or 3D fix. */
if (gps_data->status == STATUS_FIX &&
(gps_data->fix.mode == MODE_2D || gps_data->fix.mode == MODE_3D))
{
/* Fix Obtained, Set Values. */
gps_fix = gps_data->fix.mode;
gps_lon = gps_data->fix.longitude;
gps_lat = gps_data->fix.latitude;
gps_alt = gps_data->fix.altitude;
gps_sat = gps_data->satellites_visible;
gps_use = gps_data->satellites_used;
}
}
return true;
}
关闭 GPS
完成后，您需要关闭流并释放资源。
bool gps::stop()
{
if(gps_rec->stream(WATCH_DISABLE) == NULL)
{
delete gps_rec;
return false;
}
delete gps_rec;
return true;
}
就是这样，如您所见，设置和读取 GPS 设备坐标非常简单。现在您已经有了原始坐标值，您可能需要从十进制表示中解析出分量值。原始值表示度、分和秒作为单个十进制值。
解析度
度表示为原始值的整数部分，为了确定度，只需截断浮点值即可。在这里，我只是将其转换为 `int` 并返回绝对值。
/*
* degrees: converts decimal angle to degree component.
*
*/
int gps::degrees(float angle)
{
return abs((int)angle);
}
解析分
分是一度的六十分之一。一旦您从原始值中移除了度数分量，您就只剩下分和秒。要转换为分，您需要乘以 60，然后截断浮点值以去除秒。
/*
* minutes: converts decimal angle to minutes component.
*
*/
int gps::minutes(float angle)
{
int deg = degrees(angle);
int min = (int)((fabs(angle) - deg) * 60.00);
return min;
}
解析秒
秒是分钟的六十分之一。要获得分钟，您需要执行两个步骤：首先，从原始 GPS 坐标值中减去度数；然后，执行转换为分钟的步骤。现在您已经有了分钟，您可以减去整数值，再次乘以 60，这将留下您的秒分量。秒可以作为带小数部分的十进制值返回。
/*
* seconds: converts decimal angle to seconds component.
*
*/
float gps::seconds(float angle)
{
int deg = degrees(angle);
int min = minutes(angle);
float sec = (((fabs(angle) - deg) * 60.00) - (float)min) * 60.00;
return sec;
}
确定东/西和北/南
您的 GPS 坐标原始值中还包含一个附加组件。该值确定经度的东/西和纬度的北/南。这编码在 GPS 原始值的符号位中。如果您的原始值是负数，则表示西/南；如果为正数，则表示东/北。
gps_lat >= 0 ? 'N' : 'S'; // Determine North or South.
gps_lon >= 0 ? 'E' : 'W'; // Determine East or West.
示例数据
十进制纬度：-15.063888888888888
转换后的纬度：15° 3' 50" W
十进制经度：45.33416666666667
转换后的经度：45° 20' 3" N
世界磁场模型和磁偏角
现在我们有了 GPS 坐标，下一步是确定机器人的磁偏角。我们可以使用 GeographicLib 和世界磁场模型来做到这一点，但首先我们需要理解磁偏角的概念。
磁偏角
引用：https://en.wikipedia.org/wiki/Magnetic_declination
磁偏角 或 磁倾角 是水平面上磁北（指南针针尖所指方向，对应于地球磁场线的方向）与真北（沿子午线指向地理北极的方向）之间的夹角。这个角度取决于地球表面的位置，并且会随着时间而变化。
正如您所见，磁偏角只是磁北和真北之间的偏移角度。这个值会随位置和时间而变化。确保刷新此值非常重要，因为它可能在您的控制循环的帧之间变化（可能性不大但可能）。还有一个类似的称为倾角的概念，它确定了磁场在垂直平面上的角度，但我们不需要它来确定我们相对于磁北的偏移。
磁场模型
为了确定磁偏角，我们需要能够了解机器人位置处的磁场线。从描述中我们知道磁偏角会随位置和时间而变化。那么，如果我们不断变化，我们怎么知道实际的偏角是多少呢？世界磁场模型就是答案，它是一个由 NOAA 提供的预计算数据集，可以被 GeographicLib 使用。提供的数据集通常可以正常使用几年，需要定期更新以确保信息的准确性。
您可以在 NOAA 网站 上找到更多信息和有用的工具。
使用 GeographicLib
我创建了一个类似 GPS 包装器的 GeographicLib 库的简单包装器。它具有我们希望查询的属性，例如偏角和倾角。这段代码实际上比 GPS 组件更容易使用，并且不需要 `start()` 或 `stop()` 步骤。
#ifndef __WMM_H__
#define __WMM_H__
#include <GeographicLib/MagneticModel.hpp> // Magnetic Model
using namespace GeographicLib;
class wmm
{
private:
double magnetic_declination;
double magnetic_inclination;
double field_strength;
public:
double declination();
double inclination();
double strength();
void update(float lat, float lon, float alt);
};
#endif // !__WMM_H__
获取偏角
我们将需要使用 GPS 模块返回的纬度、经度和高度来调用我们的 `update(...)` 函数。这将为我们提供一个 3D 位置来确定地球产生的局部磁场。此外，我们还需要确定当前日期和时间，以返回当前时刻准确的世界磁场模型信息。
void wmm::update(float lat, float lon, float alt)
{
/* intermediate values */
double Bx, By, Bz, H;
/* Determine current time. */
time_t t = time(NULL);
tm* timePtr = localtime(&t);
/*
* Magnetic Model component, Using emm2015 magnetic model
* (emm2015 includes magnetic interactions from Earth's crust)
*/
MagneticModel mag("emm2015");
/* Get intermediate values using current time and position */
mag(timePtr->tm_year + 1900, lat, lon, alt, Bx, By, Bz);
/* Convert intermediate values into field components and store in class members */
MagneticModel::FieldComponents(Bx, By, Bz, H,
field_strength, magnetic_declination, magnetic_inclination);
}
读取指南针方向
我们的指南针使用 i2c 接口，除了 i2c-dev.h 之外不需要其他库。这使得它的设置比 GPS 稍微复杂一些，但差别不大。返回的值应该是本地磁北方向。这是我们需要偏移以确定真北方向的值。磁力计容易受到噪声干扰，并且由于局部磁场干扰，可能无法随时间返回一致的值。这可能是由于您传感器附近的其他金属或电子设备，或者在我遇到的情况下是我的跳线长度。您需要确保您的指南针传感器尽可能与随机磁场隔离。
指南针类
指南针类非常简单，它与 i2c 交互，然后使用卡尔曼滤波器过滤值以获得更平滑的输出。
#ifndef __COMPASS_H__
#define __COMPASS_H__
#include "kalman.h"
#define HMC5883L_I2C_ADDR 0x1E
class compass
{
private:
int i2c_fd;
/* i2c interaction functions */
bool select_i2c_device(int fd, int addr, char * name);
bool write_to_i2c(int fd, int reg, int val);
public:
/* Filtered Compass Bearing */
float bearing;
/* Routines for managing interactions with the compass sensor */
int start();
int update();
};
#endif
初始化指南针
为了初始化指南针，我们首先需要选择我们的 i2c 设备，然后使用以下 `select_i2c_device(...)` 和 `write_to_i2c(...)` 函数将一些数据写入总线。
/* Select our i2c compass device */
bool compass::select_i2c_device(int fd, int addr, char * name)
{
if (ioctl(fd, I2C_SLAVE, addr) < 0) {
return false;
}
return true;
}
/* Write something to the i2c device */
bool compass::write_to_i2c(int fd, int reg, int val)
{
char buf[2];
buf[0]=reg;
buf[1]=val;
if (write(fd, buf, 2) != 2) {
return false;
}
return true;
}
现在在我们的 `start()` 函数中，我们可以调用前面的 i2c 函数来设置我们的设备。
int compass::start()
{
init = true;
/* Initialize i2c compass */
if ((i2c_fd = open("/dev/i2c-1", O_RDWR)) < 0)
{
return 0;
}
/* initialise HMC5883L */
select_i2c_device(i2c_fd, HMC5883L_I2C_ADDR, "HMC5883L");
/* Set Initial register values*/
write_to_i2c(i2c_fd, 0x01, 32);
write_to_i2c(i2c_fd, 0x02, 0);
return 1;
}
读取指南针方向
现在指南针应该可以读取了。与 GPS 和 WMM 一样，我们希望持续更新我们的传感器值，以防我们的位置或方向发生变化，与我们使用的其他组件类似，我们将使用 `update()` 函数。
int compass::update()
{
float angle = 0;
unsigned char i2c_buf[16];
i2c_buf[0] = 0x03;
/* Send the register to read from */
if ((write(i2c_fd, i2c_buf, 1)) != 1)
{
return 0;
}
/* Read from the register values. */
if (read(i2c_fd, i2c_buf, 6) != 6)
{
return 0;
}
else
{
/* Convert raw byte values to shorts. */
short x = (i2c_buf[0] << 8) | i2c_buf[1];
short y = (i2c_buf[4] << 8) | i2c_buf[5];
/* Determine Compass Bearing Angle from raw x and y components. */
angle = atan2(y, x) * 180 / M_PI;
}
/* Set the filtered Bearing. */
bearing = angle;
return 1;
}
在这里，您可以看到我们正在从 i2c 缓冲区读取，然后将字节值转换为角度。您还会看到我们使用了卡尔曼滤波器。这将减少指南针传感器返回的噪声量（下一节将详细介绍）。
过滤指南针值
指南针值由于局部电磁源和物理运动的干扰而固有地存在噪声。您可能会注意到在后续读取后，您的值不会保持恒定，尽管在值之间没有大的跳跃的情况下它应该相当一致。但是，最好对这些值进行长时间平滑处理以获得更清晰的读数。有多种过滤器可以使用，但我选择了一个简单的卡尔曼滤波器，原始实现可以在 此处 找到。
引用：https://en.wikipedia.org/wiki/Kalman_filter卡尔曼滤波，也称为线性二次估计（LQE），是一种算法，它使用一系列随时间观察到的测量值，这些测量值包含统计噪声和其他不准确之处，并通过使用贝叶斯推理并为每个时间帧估计变量的联合概率分布，产生对未知变量的估计，这些估计往往比仅基于单次测量的值更精确。该滤波器以 Rudolf E. Kálmán 的名字命名，他是其理论的主要开发者之一。
过滤
下面的图表显示了滤波后的值与原始值的关系。较暗的线代表传感器返回的噪声值，较浅的线代表平滑值。如您所见，平滑值比未滤波的值更一致。滤波后的值往往随着时间的推移更均匀，重要的是要记住我们正在对我们的值应该在哪里进行预测，它可能会略微滞后于您的原始值。这意味着在处理过程中，“锁定”目标值可能需要稍长的时间。
Kalman.h
由于我们只需要存储几个值然后更新它们，因此无需创建包装类。我们有一个存储状态的结构和两个函数：`kalman_init(...)` 和 `kalman_update(...)`。
#ifndef __KALMAN_H__
#define __KALMAN_H__
typedef struct {
float q; //process noise covariance
float r; //measurement noise covariance
float x; //value
float p; //estimation error covariance
float k; //kalman gain
} kalman_state;
kalman_state kalman_init(float q, float r, float p, float x);
void kalman_update(kalman_state *s, float m);
#endif // __KALMAN_H__
初始化卡尔曼滤波器
初始化非常简单，我们创建一个新的卡尔曼状态结构，然后将状态的值设置为传递给函数的值。我们传入过程噪声协方差 (q)、测量噪声协方差 (r)、估计误差协方差 (p) 和初始值 (x)。
kalman_state kalman_init(float q, float r, float p, float x) {
kalman_state s;
s.q = q;
s.r = r;
s.p = p;
s.x = x;
return s;
}
在我们的控制循环中，我们将使用以下代码行初始化卡尔曼滤波器。我选择的这些值是估算的，我只是调整它们直到返回的值看起来相当平滑。
/* Initialize Kalman filter on first call only */
if(init)
{
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
更新卡尔曼滤波器
要进行更新，我们将传入先前状态和指南针的新测量值。首先，我们需要根据先前的状态进行预测，然后更新测量值。
void kalman_update(kalman_state * s, float m) {
//prediction update
//omit x = x
s->p = s->p + s->q;
//measurement update
s->k = s->p / (s->p + s->r);
s->x = s->x + s->k * (m - s->x);
s->p = (1 - s->k) * s->p;
}
最后，在我们的主控制循环中，每次更新指南针时，我们都会调用卡尔曼滤波器的更新函数。
/* Update Filtered Bearing */ kalman_update(&state, compass_sensor->bearing); /* Store Filtered Bearing */ float filtered_bearing = state.x;
就是这样，我们现在可以把它们放在一起，得到最终的真北方向了。
整合
一旦所有组件都准备好了，我们就可以构建我们的主控制循环了。在这个例子中，我只是在无限循环中将值打印到屏幕上。您可能需要为您的目的创建一个精心设计的控制循环。
初始化
我们首先创建并初始化 GPS、WMM、指南针和卡尔曼滤波器组件。接下来，我们在进入主循环之前调用任何 `start()` 方法。
/* Setup Kalman Filter */
bool init = true;
kalman_state state;
/* Create Components */
gps * gps_sensor = new gps();
wmm * magnetic_model = new wmm();
compass * compass_sensor = new compass();
/* Initialize Components */
gps_sensor->start();
compass_sensor->start();
控制循环
一旦进入主控制循环，您只需更新 GPS 传感器，将经度、纬度和高度值传递给世界磁场模型。现在我们从指南针读取，初始化卡尔曼滤波器，然后过滤值。
/* Enter Main Control Loop */
while (1)
{
/* Update GPS Sensor */
gps_sensor->update();
/* Pass GPS Data to WMM to get Declination */
magnetic_model->update(gps_sensor->gps_lat, gps_sensor->gps_lon, gps_sensor->gps_alt);
/* Read from Compass Sensor */
compass_sensor->update();
/* Initialize the Kalman filter */
if (init)
{
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
/* Update Filtered Bearing */
kalman_update(&state, compass_sensor->bearing);
/* Store Filtered Bearing */
float filtered_bearing = state.x;
}
真北
一旦我们获得了滤波后的指南针值，我们就可以使用以下代码行来确定与我们方向的偏移。这是一个非常简单的计算，只需将磁偏角偏移添加到您的滤波后的方向值即可。
float true_bearing = filtered_bearing + magnetic_model->declination();
打印值
在这个演示中，我只是将值打印到屏幕上。然而，在我的情况下，我使用这些值来帮助控制伺服电机，以控制我的望远镜的平移。您应该使用这些值来控制您的设备，而不是仅仅打印值，这就是您需要执行任何额外步骤的地方。
/* Reset Terminal Output */
printf("\033[0;0H");
/* Print GPS Coordinates */
printf("Latitude: %3d° %2d' %2.3f\" %c Longitude: %3d° %2d' %2.3f\" %c\n\r",
gps_sensor->latitude_degrees(),
gps_sensor->latitude_minutes(),
gps_sensor->latitude_seconds(),
gps_sensor->gps_lat >= 0 ? 'N' : 'S',
gps_sensor->longitude_degrees(),
gps_sensor->longitude_minutes(),
gps_sensor->longitude_seconds(),
gps_sensor->gps_lon >= 0 ? 'E' : 'W');
/* Print Altitude and Declination */
printf("Altitude: %2.3f Declination: %2.3f",
gps_sensor->gps_alt),
magnetic_model->declination());
/* Print Raw Bearing, Filtered Bearing and True Bearing */
printf("Magnetic Bearing: %2.3f Filtered Bearing %2.3f True Bearing %2.3f\n\r",
compass_sensor->bearing,
filtered_bearing,
true_bearing);
清理
退出主循环后，您需要记住停止 GPS 并清理您的组件。
/* Stop GPS sensor */
gps_sensor->stop();
/* Cleanup Components */
delete gps_sensor;
delete compass_sensor;
delete magnetic_model;
完整示例
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "wmm.h"
#include "compass.h"
#include "gps.h"
#include "kalman.h"
int main()
{
bool init = true;
kalman_state state;
printf("starting...\n\r");
/* Create Components */
gps * gps_sensor = new gps();
wmm * magnetic_model = new wmm();
compass * compass_sensor = new compass();
/* Initialize Components */
gps_sensor->start();
compass_sensor->start();
/* Clear Screen */
printf("\033[2J\033[?25l");
/* Enter Main Control Loop */
while (1) {
/* Update GPS Sensor */
gps_sensor->update();
/* Pass GPS Data to WMM to get Declination */
magnetic_model->update(gps_sensor->gps_lat, gps_sensor->gps_lon, gps_sensor->gps_alt);
/* Read from Compass Sensor */
compass_sensor->update();
if (init) {
state = kalman_init(0.025f, 16, 1, compass_sensor->bearing);
init = false;
}
kalman_update(&state, compass_sensor->bearing);
float filtered_bearing = state.x;
/* Calculate True Bearing from compass bearing and WMM Declination */
float true_bearing = filtered_bearing + magnetic_model->declination();
/* Reset Terminal Output */
printf("\033[0;0H");
/* Print GPS Coordinates */
printf("Latitude: %3d° %2d' %2.3f\" %c\n\rLongitude: %3d° %2d' %2.3f\" %c\n\r",
gps_sensor->latitude_degrees(),
gps_sensor->latitude_minutes(),
gps_sensor->latitude_seconds(),
gps_sensor->gps_lat >= 0 ? 'N' : 'S',
gps_sensor->longitude_degrees(),
gps_sensor->longitude_minutes(),
gps_sensor->longitude_seconds(),
gps_sensor->gps_lon >= 0 ? 'E' : 'W');
/* Print Altitude and Declination */
printf("Altitude: %2.3f\n\r\n\rDeclination: %2.3f\n\r",
gps_sensor->gps_alt,
magnetic_model->declination());
/* Print Raw Bearing, Filtered Bearing and True Bearing */
printf("\n\rMagnetic Bearing: %2.3f\n\rFiltered Bearing: %2.3f\n\rTrue Bearing %2.3f\n\r",
compass_sensor->bearing,
filtered_bearing,
true_bearing);
}
gps_sensor->stop();
delete gps_sensor;
delete compass_sensor;
delete magnetic_model;
return 0;
}
Using the Code
如果您按照图表和文章开头的软件先决条件设置了硬件，则可以使用以下 bash 命令下载并运行示例。
git clone https://github.com/GalacticSoft/Starbot-Command.git
cd Starbot-Command/src
make clean
make demo
sudo ./demo
就是这样，如果演示成功，您应该会在屏幕上看到您的坐标，如下面的截图所示。
现在您可以开始在您的机器人或物联网设备中使用地理位置和指南针方向了，祝您愉快！