宠物活动轨迹追踪系统：GPS/BDS+UWB+BLE多定位融合方案
摘要：本文深入讲解宠物活动轨迹追踪系统的多定位融合技术，涵盖GPS/BDS室外定位、UWB室内定位、BLE信标定位，以及地理围栏、轨迹分析等核心功能。
一、定位技术选型
1.1 室外定位技术对比
| 技术 | 精度 | 功耗 | 成本 | 适用场景 |
|---|---|---|---|---|
| GPS L1 | 2.5m | 25mA | ¥22 | 室外开阔地 |
| GPS L1+L5 | 1.0m | 30mA | ¥35 | 高精度室外 |
| BDS B1I | 2.5m | 25mA | ¥22 | 亚太地区 |
| GPS+BDS双模 | 1.5m | 28mA | ¥28 | 全球覆盖 |
| A-GPS | 5m | 10mA | ¥15 | 快速定位 |
推荐方案：LC29H（Quectel）- GPS+BDS+GLONASS三模
1.2 室内定位技术对比
| 技术 | 精度 | 功耗 | 成本 | 适用场景 |
|---|---|---|---|---|
| UWB | 10cm | 30mA | ¥50 | 高精度室内 |
| BLE 5.1 AoA | 0.5m | 8mA | ¥20 | 中精度室内 |
| BLE RSSI | 2-3m | 5mA | ¥8 | 低精度室内 |
| WiFi RTT | 1-2m | 15mA | ¥12 | 家庭环境 |
推荐方案：DW3000（Qorvo）- UWB高精度定位
1.3 融合定位架构
┌─────────────────────────────────────────────────┐
│ 定位融合引擎 │
│ ┌──────────┐ ┌──────────┐ ┌──────────┐ │
│ │ GPS/BDS │ │ UWB │ │ BLE RSSI │ │
│ │ 室外定位 │ │ 室内定位 │ │ 区域定位 │ │
│ └────┬─────┘ └────┬─────┘ └────┬─────┘ │
│ │ │ │ │
│ ▼ ▼ ▼ │
│ ┌─────────────────────────────────────────┐ │
│ │ 卡尔曼滤波融合 │ │
│ └─────────────────────────────────────────┘ │
│ │ │
│ ▼ │
│ ┌─────────────────────────────────────────┐ │
│ │ 位置平滑与轨迹生成 │ │
│ └─────────────────────────────────────────┘ │
└─────────────────────────────────────────────────┘二、GPS/BDS室外定位
2.1 NMEA数据解析
c
// NMEA解析器
typedef struct {
double latitude; // 纬度（度）
double longitude; // 经度（度）
float altitude; // 海拔（米）
float speed; // 速度（km/h）
float course; // 航向（度）
uint8_t satellites; // 可见卫星数
float hdop; // 水平精度因子
uint8_t fix_quality; // 定位质量
uint32_t timestamp; // UTC时间戳
bool valid; // 数据有效标志
} gps_data_t;
// 解析GGA语句
bool parse_gga(const char *nmea, gps_data_t *data) {
// $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
if (strncmp(nmea, "$GPGGA", 6) != 0 &&
strncmp(nmea, "$GNGGA", 6) != 0) {
return false;
}
char *token = strtok((char *)nmea, ",");
int field = 0;
while (token != NULL) {
switch (field) {
case 1: // 时间
parse_time(token, &data->timestamp);
break;
case 2: // 纬度
data->latitude = parse_latitude(token);
break;
case 3: // N/S
if (token[0] == 'S') data->latitude = -data->latitude;
break;
case 4: // 经度
data->longitude = parse_longitude(token);
break;
case 5: // E/W
if (token[0] == 'W') data->longitude = -data->longitude;
break;
case 6: // 定位质量
data->fix_quality = atoi(token);
break;
case 7: // 卫星数
data->satellites = atoi(token);
break;
case 8: // HDOP
data->hdop = atof(token);
break;
case 9: // 海拔
data->altitude = atof(token);
break;
}
token = strtok(NULL, ",");
field++;
}
data->valid = (data->fix_quality > 0);
return data->valid;
}2.2 低功耗定位策略
c
typedef enum {
GPS_MODE_OFF, // 关闭
GPS_MODE_COLD_START, // 冷启动
GPS_MODE_HOT_START, // 热启动
GPS_MODE_CONTINUOUS, // 连续定位
GPS_MODE_POWER_SAVE, // 省电模式
GPS_MODE_PERIODIC // 周期定位
} gps_mode_t;
typedef struct {
gps_mode_t mode;
uint32_t interval_ms; // 定位间隔
uint32_t timeout_ms; // 定位超时
uint32_t last_fix_time;
bool has_fix;
gps_data_t last_position;
} gps_controller_t;
void gps_set_mode(gps_controller_t *ctrl, gps_mode_t mode) {
ctrl->mode = mode;
switch (mode) {
case GPS_MODE_OFF:
gps_power_off();
break;
case GPS_MODE_COLD_START:
gps_power_on();
gps_send_command("$PMTK104"); // 全冷启动
ctrl->interval_ms = 0;
ctrl->timeout_ms = 120000; // 2分钟超时
break;
case GPS_MODE_HOT_START:
gps_power_on();
gps_send_command("$PMTK101"); // 热启动
ctrl->interval_ms = 0;
ctrl->timeout_ms = 30000; // 30秒超时
break;
case GPS_MODE_CONTINUOUS:
gps_power_on();
gps_send_command("$PMTK220,1000"); // 1秒更新
ctrl->interval_ms = 1000;
break;
case GPS_MODE_POWER_SAVE:
gps_power_on();
gps_send_command("$PMTK220,5000"); // 5秒更新
ctrl->interval_ms = 5000;
break;
case GPS_MODE_PERIODIC:
gps_power_on();
gps_send_command("$PMTK220,60000"); // 60秒更新
ctrl->interval_ms = 60000;
break;
}
}
// 根据活动状态自适应调整
void gps_adaptive_mode(gps_controller_t *ctrl, pet_activity_t activity) {
switch (activity) {
case ACTIVITY_SLEEPING:
gps_set_mode(ctrl, GPS_MODE_OFF);
break;
case ACTIVITY_LOW:
gps_set_mode(ctrl, GPS_MODE_PERIODIC);
break;
case ACTIVITY_HIGH:
gps_set_mode(ctrl, GPS_MODE_CONTINUOUS);
break;
case ACTIVITY_LOST:
gps_set_mode(ctrl, GPS_MODE_CONTINUOUS);
break;
}
}三、UWB室内定位
3.1 双边测距(TWR)原理
锚点A 标签(宠物)
│ │
│──── Poll (T1) ────────→│
│ │
│←── Response (T2) ──────│
│ │
│──── Final (T3) ───────→│
│ │
└─────────────────────────┘
距离 = c × ((T3-T1) - (T2-T1)) / 2
其中 c = 光速3.2 TWR测距代码
c
// DW3000 TWR测距
typedef struct {
uint64_t poll_tx_time;
uint64_t poll_rx_time;
uint64_t response_tx_time;
uint64_t response_rx_time;
uint64_t final_tx_time;
uint64_t final_rx_time;
} twr_timestamps_t;
float calculate_distance(twr_timestamps_t *ts) {
// 时间差计算
int64_t round1 = ts->response_rx_time - ts->poll_tx_time;
int64_t reply1 = ts->response_tx_time - ts->poll_rx_time;
int64_t round2 = ts->final_rx_time - ts->response_tx_time;
int64_t reply2 = ts->final_tx_time - ts->response_rx_time;
// 距离计算
float tof = ((round1 * round2) - (reply1 * reply2)) /
(float)(round1 + round2 + reply1 + reply2);
// 转换为米
float distance = tof * SPEED_OF_LIGHT / 2;
// 校准补偿
distance -= ANTENNA_DELAY_CALIBRATION;
return distance;
}3.3 三边定位算法
c
typedef struct {
float x, y, z; // 锚点坐标
float distance; // 测量距离
} anchor_t;
// 最小二乘法定位
bool trilateration(anchor_t *anchors, int num_anchors,
float *result_x, float *result_y) {
if (num_anchors < 3) return false;
// 构建方程组 Ax = b
float A[MAX_ANCHORS-1][2];
float b[MAX_ANCHORS-1];
for (int i = 1; i < num_anchors; i++) {
A[i-1][0] = 2 * (anchors[i].x - anchors[0].x);
A[i-1][1] = 2 * (anchors[i].y - anchors[0].y);
b[i-1] = (anchors[0].distance * anchors[0].distance -
anchors[i].distance * anchors[i].distance +
anchors[i].x * anchors[i].x - anchors[0].x * anchors[0].x +
anchors[i].y * anchors[i].y - anchors[0].y * anchors[0].y);
}
// 最小二乘解：x = (A^T A)^-1 A^T b
float AT[2][MAX_ANCHORS-1];
float ATA[2][2];
float ATb[2];
transpose(A, AT, num_anchors-1, 2);
mat_mult(AT, A, ATA, 2, num_anchors-1, 2);
mat_mult(AT, b, ATb, 2, num_anchors-1, 1);
// 2x2矩阵求逆
float det = ATA[0][0] * ATA[1][1] - ATA[0][1] * ATA[1][0];
if (fabs(det) < 1e-6) return false;
float inv[2][2];
inv[0][0] = ATA[1][1] / det;
inv[0][1] = -ATA[0][1] / det;
inv[1][0] = -ATA[1][0] / det;
inv[1][1] = ATA[0][0] / det;
*result_x = inv[0][0] * ATb[0] + inv[0][1] * ATb[1];
*result_y = inv[1][0] * ATb[0] + inv[1][1] * ATb[1];
return true;
}四、BLE信标定位
4.1 RSSI测距模型
c
// RSSI转距离
float rssi_to_distance(int rssi, int tx_power, float path_loss_exp) {
// 对数距离路径损耗模型
// d = 10 ^ ((TxPower - RSSI) / (10 * n))
// n: 路径损耗指数（2.0-4.0）
float ratio = (tx_power - rssi) / (10.0 * path_loss_exp);
return pow(10, ratio);
}
// 卡尔曼滤波平滑RSSI
typedef struct {
float x; // 状态
float P; // 协方差
float Q; // 过程噪声
float R; // 测量噪声
} rssi_kalman_t;
int rssi_kalman_update(rssi_kalman_t *kf, int rssi_measurement) {
// 预测
float x_pred = kf->x;
float P_pred = kf->P + kf->Q;
// 更新
float K = P_pred / (P_pred + kf->R);
kf->x = x_pred + K * (rssi_measurement - x_pred);
kf->P = (1 - K) * P_pred;
return (int)kf->x;
}4.2 BLE指纹定位
c
// 离线阶段：采集指纹库
typedef struct {
float x, y; // 位置坐标
int rssi[NUM_BEACONS]; // 各信标RSSI
} fingerprint_t;
typedef struct {
fingerprint_t database[MAX_FINGERPRINTS];
int count;
} fingerprint_db_t;
// 在线阶段：KNN匹配
void ble_fingerprint_localize(fingerprint_db_t *db,
int *current_rssi,
int k,
float *result_x, float *result_y) {
// 计算与各指纹的距离
float distances[MAX_FINGERPRINTS];
for (int i = 0; i < db->count; i++) {
float dist = 0;
for (int j = 0; j < NUM_BEACONS; j++) {
float diff = current_rssi[j] - db->database[i].rssi[j];
dist += diff * diff;
}
distances[i] = sqrt(dist);