宠物活动轨迹追踪系统GPS/BDSUWBBLE多定位融合方案摘要本文深入讲解宠物活动轨迹追踪系统的多定位融合技术涵盖GPS/BDS室外定位、UWB室内定位、BLE信标定位以及地理围栏、轨迹分析等核心功能。一、定位技术选型1.1 室外定位技术对比技术精度功耗成本适用场景GPS L12.5m25mA¥22室外开阔地GPS L1L51.0m30mA¥35高精度室外BDS B1I2.5m25mA¥22亚太地区GPSBDS双模1.5m28mA¥28全球覆盖A-GPS5m10mA¥15快速定位推荐方案LC29HQuectel- GPSBDSGLONASS三模1.2 室内定位技术对比技术精度功耗成本适用场景UWB10cm30mA¥50高精度室内BLE 5.1 AoA0.5m8mA¥20中精度室内BLE RSSI2-3m5mA¥8低精度室内WiFi RTT1-2m15mA¥12家庭环境推荐方案DW3000Qorvo- UWB高精度定位1.3 融合定位架构┌─────────────────────────────────────────────────┐ │ 定位融合引擎 │ │ ┌──────────┐ ┌──────────┐ ┌──────────┐ │ │ │ GPS/BDS │ │ UWB │ │ BLE RSSI │ │ │ │ 室外定位 │ │ 室内定位 │ │ 区域定位 │ │ │ └────┬─────┘ └────┬─────┘ └────┬─────┘ │ │ │ │ │ │ │ ▼ ▼ ▼ │ │ ┌─────────────────────────────────────────┐ │ │ │ 卡尔曼滤波融合 │ │ │ └─────────────────────────────────────────┘ │ │ │ │ │ ▼ │ │ ┌─────────────────────────────────────────┐ │ │ │ 位置平滑与轨迹生成 │ │ │ └─────────────────────────────────────────┘ │ └─────────────────────────────────────────────────┘二、GPS/BDS室外定位2.1 NMEA数据解析// NMEA解析器typedefstruct{doublelatitude;// 纬度度doublelongitude;// 经度度floataltitude;// 海拔米floatspeed;// 速度km/hfloatcourse;// 航向度uint8_tsatellites;// 可见卫星数floathdop;// 水平精度因子uint8_tfix_quality;// 定位质量uint32_ttimestamp;// UTC时间戳bool valid;// 数据有效标志}gps_data_t;// 解析GGA语句boolparse_gga(constchar*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*hhif(strncmp(nmea,$GPGGA,6)!0strncmp(nmea,$GNGGA,6)!0){returnfalse;}char*tokenstrtok((char*)nmea,,);intfield0;while(token!NULL){switch(field){case1:// 时间parse_time(token,data-timestamp);break;case2:// 纬度data-latitudeparse_latitude(token);break;case3:// N/Sif(token[0]S)data-latitude-data-latitude;break;case4:// 经度data-longitudeparse_longitude(token);break;case5:// E/Wif(token[0]W)data-longitude-data-longitude;break;case6:// 定位质量data-fix_qualityatoi(token);break;case7:// 卫星数data-satellitesatoi(token);break;case8:// HDOPdata-hdopatof(token);break;case9:// 海拔data-altitudeatof(token);break;}tokenstrtok(NULL,,);field;}data-valid(data-fix_quality0);returndata-valid;}2.2 低功耗定位策略typedefenum{GPS_MODE_OFF,// 关闭GPS_MODE_COLD_START,// 冷启动GPS_MODE_HOT_START,// 热启动GPS_MODE_CONTINUOUS,// 连续定位GPS_MODE_POWER_SAVE,// 省电模式GPS_MODE_PERIODIC// 周期定位}gps_mode_t;typedefstruct{gps_mode_tmode;uint32_tinterval_ms;// 定位间隔uint32_ttimeout_ms;// 定位超时uint32_tlast_fix_time;bool has_fix;gps_data_tlast_position;}gps_controller_t;voidgps_set_mode(gps_controller_t*ctrl,gps_mode_tmode){ctrl-modemode;switch(mode){caseGPS_MODE_OFF:gps_power_off();break;caseGPS_MODE_COLD_START:gps_power_on();gps_send_command($PMTK104);// 全冷启动ctrl-interval_ms0;ctrl-timeout_ms120000;// 2分钟超时break;caseGPS_MODE_HOT_START:gps_power_on();gps_send_command($PMTK101);// 热启动ctrl-interval_ms0;ctrl-timeout_ms30000;// 30秒超时break;caseGPS_MODE_CONTINUOUS:gps_power_on();gps_send_command($PMTK220,1000);// 1秒更新ctrl-interval_ms1000;break;caseGPS_MODE_POWER_SAVE:gps_power_on();gps_send_command($PMTK220,5000);// 5秒更新ctrl-interval_ms5000;break;caseGPS_MODE_PERIODIC:gps_power_on();gps_send_command($PMTK220,60000);// 60秒更新ctrl-interval_ms60000;break;}}// 根据活动状态自适应调整voidgps_adaptive_mode(gps_controller_t*ctrl,pet_activity_tactivity){switch(activity){caseACTIVITY_SLEEPING:gps_set_mode(ctrl,GPS_MODE_OFF);break;caseACTIVITY_LOW:gps_set_mode(ctrl,GPS_MODE_PERIODIC);break;caseACTIVITY_HIGH:gps_set_mode(ctrl,GPS_MODE_CONTINUOUS);break;caseACTIVITY_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测距代码// DW3000 TWR测距typedefstruct{uint64_tpoll_tx_time;uint64_tpoll_rx_time;uint64_tresponse_tx_time;uint64_tresponse_rx_time;uint64_tfinal_tx_time;uint64_tfinal_rx_time;}twr_timestamps_t;floatcalculate_distance(twr_timestamps_t*ts){// 时间差计算int64_tround1ts-response_rx_time-ts-poll_tx_time;int64_treply1ts-response_tx_time-ts-poll_rx_time;int64_tround2ts-final_rx_time-ts-response_tx_time;int64_treply2ts-final_tx_time-ts-response_rx_time;// 距离计算floattof((round1*round2)-(reply1*reply2))/(float)(round1round2reply1reply2);// 转换为米floatdistancetof*SPEED_OF_LIGHT/2;// 校准补偿distance-ANTENNA_DELAY_CALIBRATION;returndistance;}3.3 三边定位算法typedefstruct{floatx,y,z;// 锚点坐标floatdistance;// 测量距离}anchor_t;// 最小二乘法定位booltrilateration(anchor_t*anchors,intnum_anchors,float*result_x,float*result_y){if(num_anchors3)returnfalse;// 构建方程组 Ax bfloatA[MAX_ANCHORS-1][2];floatb[MAX_ANCHORS-1];for(inti1;inum_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].distanceanchors[i].x*anchors[i].x-anchors[0].x*anchors[0].xanchors[i].y*anchors[i].y-anchors[0].y*anchors[0].y);}// 最小二乘解x (A^T A)^-1 A^T bfloatAT[2][MAX_ANCHORS-1];floatATA[2][2];floatATb[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矩阵求逆floatdetATA[0][0]*ATA[1][1]-ATA[0][1]*ATA[1][0];if(fabs(det)1e-6)returnfalse;floatinv[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_xinv[0][0]*ATb[0]inv[0][1]*ATb[1];*result_yinv[1][0]*ATb[0]inv[1][1]*ATb[1];returntrue;}四、BLE信标定位4.1 RSSI测距模型// RSSI转距离floatrssi_to_distance(intrssi,inttx_power,floatpath_loss_exp){// 对数距离路径损耗模型// d 10 ^ ((TxPower - RSSI) / (10 * n))// n: 路径损耗指数2.0-4.0floatratio(tx_power-rssi)/(10.0*path_loss_exp);returnpow(10,ratio);}// 卡尔曼滤波平滑RSSItypedefstruct{floatx;// 状态floatP;// 协方差floatQ;// 过程噪声floatR;// 测量噪声}rssi_kalman_t;intrssi_kalman_update(rssi_kalman_t*kf,intrssi_measurement){// 预测floatx_predkf-x;floatP_predkf-Pkf-Q;// 更新floatKP_pred/(P_predkf-R);kf-xx_predK*(rssi_measurement-x_pred);kf-P(1-K)*P_pred;return(int)kf-x;}4.2 BLE指纹定位// 离线阶段采集指纹库typedefstruct{floatx,y;// 位置坐标intrssi[NUM_BEACONS];// 各信标RSSI}fingerprint_t;typedefstruct{fingerprint_tdatabase[MAX_FINGERPRINTS];intcount;}fingerprint_db_t;// 在线阶段KNN匹配voidble_fingerprint_localize(fingerprint_db_t*db,int*current_rssi,intk,float*result_x,float*result_y){// 计算与各指纹的距离floatdistances[MAX_FINGERPRINTS];for(inti0;idb-count;i){floatdist0;for(intj0;jNUM_BEACONS;j){floatdiffcurrent_rssi[j]-db-database[i].rssi[j];distdiff*diff;}distances[i]sqrt(dist);
)
