方法概述

UWB 主要应用于室内定位，GPS 主要应用于室外定位。因为在室内往往 GPS 信号很差导致 GPS 相关代码在室内用不上，尝试用 UWB 位置数据来替代 GPS 来进行室内导航。这样做把已有的 GPS 相关代码应用起来，避免从算法入手而引起对代码的大面积修改。
再偷懒一点，UWB 买的是研创物联的，定位代码是现成的，再花点钱买他们的上位机软件，协议报文解析代码也到手了。现在等于直接拿到了可用于替换 GPS 的位置信息。下面就做个代码搬运就行了，所以本文就是个技能类推文。

方法实操

 以研创物联的 UWB 为例进行说明。

一、将 UWB 接到飞控的串口上
TELEM1 或 TELEM2 随意，接好后调整串口协议参数，如下：

二、按照 ardupilot 的前后台架构加入自己 UWB 相关代码

这部分程序的作用是按协议解析出 UWB 输出的相对位置，以 cm 为单位。

三、在 AP_GPS 里加一个数据来源于 UWB 的 GPS 设备

这部分程序的作用是把 UWB 的相对位置转换成经纬度。

UWB 相对位置转经纬度方法

1、地球半径 6378137 米
2、1 纬度对应的米数

1*(pi()/180)*6378137 = 111319.490793274 1*1e7*1e-7*6378137*(pi()/180) = 111319.490793274 1e-7*6378137*(pi()/180) = 0.0111319490793274

3、纬度差对应的米数

(纬度差*1e7) * 0.0111319490793274 = 纬度差对应的米数

4、如何得到“目标纬度”

(目标纬度 * 1e7 - 初始纬度 * 1e7) * 0.0111319490793274 = 纬度差对应的米数 目标纬度 * 1e7 = 初始纬度 * 1e7 + 纬度差对应的米数 / 0.0111319490793274

由此可见我们只要从参数里设好初始纬度就能把 UWB 的相对位置转换成目标纬度了。
5、目标经度与当地纬度的关系

通过上面两图不难发现：相同的经度差对应的距离差在地球不同的位置是不同的，需要用当地的纬度修正一下。在 ArduPilot 里有现成的修正方法，可以自己照样写个接口，如下：

AP_Beacon::get_singleton()->get_origin(origin_loc);AP_Beacon::get_singleton()->get_vehicle_position_ned(position, accuracy_estimate); state.location.lat = origin_loc.lat + ((double)position.x * (double)LOCATION_SCALING_FACTOR_INV);state.location.lng = origin_loc.lng + ((double)position.y * (double)LOCATION_SCALING_FACTOR_INV) / (double)longitude_scale(origin_loc); 

方法调试

1、拿着 UWB 标签行走，记录x、y、z的相对位置，确认数据正确（量和单位）。

2、调整 UWB 朝向参数，使 UWB 相对位置与 GPS 经纬度转出的位置重合。

到这个时候还不能用来导航，因为 GPS 除了报出经纬度还会报出经纬度方向的速度，而 UWB 只有位置没有速度，这样与 ArduPilot 原有的数据融合算法就对不上了。我们可以用位置的微分当作速度，如下：

if(fabsf((double)position.x - _last_x_m) > 0.03f || fabsf((double)position.y - _last_y_m) > 0.03f){ double pass_time_s_inv = (double)1000.0f / (double)(AP_HAL::millis() - _last_pos_time);  state.velocity.x = ((double)position.x - _last_x_m) * pass_time_s_inv; state.velocity.y = ((double)position.y - _last_y_m) * pass_time_s_inv; state.ground_speed = sqrtf(sq(state.velocity.x) + sq(state.velocity.y)); state.have_speed_accuracy = true; state.speed_accuracy = 0.5f;  _last_x_m = (double)position.x; _last_y_m = (double)position.y; _last_pos_time = AP_HAL::millis();}

至此就可以用 ArduPilot 原有的数据融合方法使用 UWB 数据进行粗略的室内位置导航了。

技术技能与方法论

技术研发向来是“方法论”与“技术技能”并行发展的过程。在没有足够的“技术技能”之前学习“方法论”的结果通常是：原来如此简单，但就是做不出来。在掌握“方法论”之前学习“技术技能”通常会存疑：学这么多玩儿意儿有啥用，又多又枯燥好烦哪。

Sugar 推文特色是将“方法论”和“技术技能”有节奏地划分。推荐先掌握一定的“技术技能”，在量的积累到达一定程度的时候接触相应级别的“方法论”。姿态解算算法系列推文还差两个部分就到一小段落了，分别是：进阶算法的互补滤波、进阶算法的线性 Kalman 滤波。就以姿态解算算法为例，下面 说一说相关的技术技能与方法论是怎样统一的。

一直跟着 推文的读者都知道，从去年 6 月起到现在，除了最近姿态解算类推文是重在讲“算法”的之外，其余都偏重于“编程技能类”推文，这与 上面说的“先掌握技能”一致。控制技术的相关技能可以划分为两大部分：算法研究和编程能力。其中编程能力是最基本的，所以 先结合 RT-Thread 做了软件架构训练，先把编程技能推到需要的高度。在很多读者的支持下给自己家两岁宝宝的生日礼物“小麦轮车”演变成了承载方法论和技术技能的实体。

这个开源项目结合的编程技能有（只挑出部分）：
1、ArduPilot 软件架构；
2、RT-Thread 操作系统；
3、ArduPilot 实时数据采集方法(文件系统应用)；
4、mavlink 协议通信；
5、ESP8266 Wifi UDP 数据传输；
7、MATLAB UDP 数据接收；
8、nRF24L01 无线通信；
9、ArduPilot 数学库的移植与使用；
10、project generator 减小多平台软件维护成本。

这个开源项目结合的算法有：
1、ArduPilot 当前最新 PID 控制算法移植与应用；
2、麦克纳姆轮的运动分解方法。

这些推文的安排是在“方法论”作用下的，用最简单的遥控车形式承载这个方法论：先有看到真实数据的能力，依此为基础为算法的应用和调试打开空间。