在 ardupilot 里接入自己 uwb 最愉懒的办法

15914078741

背景介绍：
Sugar 最近推了一系列的 IMU 姿态解算类文章，姿态解算是非常基础的算法，哪怕推的“进阶算法”在位姿解算算法的范畴下也属于最基础、简单的算法。
这里提到了“位姿解算” Sugar 短期内不会做系列推文，但未来肯定会推。因为此类算法对数学要求比较高：高等数学范围内至少要知道“多变量微积分里的偏微分”、“二元函数的 Taylor 级数展开”；滤波算法上要有单传感器的“扩展卡尔曼滤波 EKF”和“无迹卡尔曼滤波 UKF”的经验；对于位姿解算还要有多传感器数据融合经验。
有没有被吓到？其实不用怕，Sugar 一如既往地从入门级别开推，一点一点把各种知识融进去就不难懂了。Sugar 有计划在推文的时候把这些算法的数学基础都逐一用最易懂的方式表述一遍。
本文在利用 ardupilot 原有算法的基础上，给出一种快速接入 UWB 数据进行位姿解算的方法。纯属编程技能类推文，不涉及复杂算法。定位精度肯定没有从算法入手高，但做为打消对算法恐惧感的“先验实践”还是可以的。

方法概述

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

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

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

这个开源项目结合的编程技能有（只挑出部分）：
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、麦克纳姆轮的运动分解方法。

这些推文的安排是在“方法论”作用下的，用最简单的遥控车形式承载这个方法论：先有看到真实数据的能力，依此为基础为算法的应用和调试打开空间。在 Sugar 的推文节奏里“方法论”主要作用是规划与统筹，把时间线安排出来，而“技术技能”主要用来填充时间线上各个节点的具体内容。
关注作者

欢迎扫码关注我的公众号MultiMCU EDU。

提示：在公众号“关于我”页面可加作者微信好友。

喜欢本文求点赞，有打赏我会更有动力。