基于UWB的室内定位无人机编队

UWB定位技术
- 测距原理
- - 单向双向测距（SS-TWR）
  - 双向双向测距（DS-TWR）
- 三基站一标签通讯图解
- 定位算法
- 模块选择
无人机接收控制指令
无人机定点飞行控制
上位机开发

先展示一下效果：

三机定位

编队飞行

UWB定位技术

超宽带无线通信技术（UWB） 是一种无载波通信技术，UWB不使用载波，而是使用短的能量脉冲序列，并通过正交频分调制或直接排序将脉冲扩展到一个频率范围内。IR-UWB信号，仅需要产生一个时间短至nS级以下的脉冲，便可通过天线进行发送。需要传送信息可以通过改变脉冲的幅度，时间，相位进行加载，进而实现信息传输。（参考于：https://zhuanlan.zhihu.com/p/138505840）

测距原理

单向双向测距（SS-TWR）

涉及单个消息从一个节点到另一个节点的往返延迟的简单测量，以及将响应发送回原始节点。

但这种测距方式的误差会随着时钟偏移增加，飞行时间估计中的误差增加到误
差使得估计非常不准确的程度。

双向双向测距（DS-TWR）

是基本的单向双向测距的延伸，其中使用了两个往返时间测量，并将其组合在一起，从而得到飞行时间结果。

在三个和四个消息情况下的所得飞行时间估计 Tprop 可以使用以下表达式来计算：

使用 DS 测距方式时钟引入的误差为：
假设设备 A 和设备 B 的时钟精度是 20ppm（很差），1ppm 为百万分之一，那么 Ka 和 Kb分别是 0.99998 或者 1.00002，ka 和 kb 分别是设备 A、B 时钟的实际频率和预期频率的比值。设备 A、B 相距 100m，电磁波的飞行时间是 333ns。则因为时钟引入的误差为 2033310-9 秒，导致测距误差为 2.2mm，可以忽略不计了。因此双边测距是最常采用的测距方式。

三基站一标签通讯图解

以主动扫描标签方式构建，所有行为集中在主基站（A 基站）上发送命令并处理，并且由非常大的可操控性，所有的执行行为只需要操控配置主基站即可。

定位算法

假设基站 A 的坐标为（x1，y1，z1），基站 B 的坐标为（x2，y2，z2），基站 C 的坐标为（x3，y3，z3）, 基站 D 的坐标为（x4，y4，z4）,需要求解的标签坐标为（x，y，z），则有：

把这些式子转换为矩阵相乘的形式

模块选择

我使用的是广州联网科技有限公司的定位系统，基于该系统，自行设计适合MiniFly的定位标签。该系统的标签是基于 DW1000 定位模块的。
并改造一下MiniFly，将716改为720，桨叶改大一点，并倒置电机，提高升力，并同时将电池换为850mah，通过这些改进，MiniFly在搭载了UWB模块之后还能有接近10分钟的续航时间。

无人机接收控制指令

要实现室内定位无人机编队，除了获取到无人机的准确位置外，还需要对无人机进行控制，最简单的控制方式就是无人机只是搭载UWB定位标签，实现定位功能，然后再电脑上位机通过串口控制遥控器进而实现无人机的定点飞行，另一种方式就是直接使用搭载在MiniFly上的UWB标签模块与无人机直接通信，从而控制无人机。
第一种方式的优点是简单快捷，而且之前也有串口上位机控制遥控从而控制无人机的基础，缺点是流程过于繁杂，会造成定位数据的延迟，在多台无人机的情况下，定位效果不佳。
第二种方式的优点是流程直接，控制数据能快速的发送到无人机上，而且无人机在获取了自己的位置之后可以采用自主飞行的方式，可以实现多台无人机的定位编队控制。缺点是代码比较复杂，定点飞行算法也需要优化。

为了实现多台无人机编队，因此我选择了第二种方式。使UWB标签模块模仿MiniFly的WiFi模块发送数据，发送的数据有多种格式，如下：
1.当收到基站发来的数据透传指令时，直接转发给MiniFly
2.在空闲时段，UWB将持续发送当前位置信息给MiniFly
3.在收到灯光控制信息时，由UWB控制WS2812的LED模块
到了MiniFly这边就接收到搭载在机身上的UWB标签模块的信息。

并在成功接收到数据后调用UWBDataHandle函数处理，这部分函数就涉及接下来的无人机定点飞行。

无人机定点飞行控制

该部分代码涉及了无人机的定点飞行，以及上位机控制无人机飞行，由于只是一个demo，只是简单的引入了误差量，还没实现PID控制。
下面展示一些 内联代码片。

static void UWBDataHandle(u8 *data)
{static u8 Fly_Speed = 20;       //飞行速度(148-128)*0.25static u8 Error = 5;            //UWB目标点与实时坐标之间的误差static u8 Deviation = 10;       //重新执行定点飞行的偏移量static bool re_arrival_fly = false;//重新执行目标点飞行标志static u8 count = 1;            //计算偏离目标点的次数（消除抖动）static u8 countfly = 1;         //用于指令控制无人机飞行的延迟//2022/06/27static bool Low_Speed_Flag = false;//用于降低飞行的速度标志u16 DiffX = 0,DiffY = 0;        //用于记录当前位置与目标点的距离static u8 Radius = 40;          //圆域半径static u8 Low_Speed = 10 , High_Speed = 20;//------------------------------------------------------------//2022/6/3接收到20条指令，或者在执行目标点飞行，就把飞行控制数据重置if(++countfly > 20 || !Arrival_Flag){UWBCtrl.pitch   = 0;UWBCtrl.roll    = 0;UWBCtrl.yaw     = 0;//航向控制数据UWBCtrl.thrust  = 32768;//油门控制数据，居中countfly = 1;}if(data[1] == 0x01) { //目标点数据if(data[2]*256+data[3] != DestX || data[4]*256+data[5] != DestY) {//如果获取到的目标点数据与当前设置的目标点数据不一致DestX = data[2]*256+data[3];//赋值DestY = data[4]*256+data[5];FirstOrder = true;Arrival_Flag = false;//修改标志位，继续飞行ArrivalX = false;ArrivalY = false;re_arrival_fly = false;Low_Speed_Flag = false;//重新发送目标点恢复速度}}else if(data[1] == 0x02) { //实时坐标数据//默认机头方向与Y轴平行LocatX = data[2]*256+data[3];LocatY = data[4]*256+data[5];//2022/06/27计算目标点与实时坐标的距离DiffX = Diff(LocatX,DestX);DiffY = Diff(LocatY,DestY);//2022/06/27到达以目标点为圆心，50为半径的圆域内，降速为10 DiffX <= Radius && DiffY <= Radiusif((DiffX * DiffX + DiffY * DiffY) <= Radius * Radius){Low_Speed_Flag = true;}else{Low_Speed_Flag = false;}//2022/06/27添加if(Low_Speed_Flag && Fly_Speed == High_Speed){//到达圆域内且飞行速度是20，防止多次调用赋值Fly_Speed = Low_Speed;}else if(Fly_Speed == Low_Speed && !Low_Speed_Flag){Fly_Speed = High_Speed;}Type = Fly_Speed;//将数据类型回传//FlagData = Low_Speed_Flag;if(!Arrival_Flag && FirstOrder) { //如果还没到达目标点//横滚飞行控制数据if(!ArrivalX) {//目标点 + 误差 > 当前坐标 && 目标点 - 误差 < 当前坐标//if((DestX + Error) > LocatX && (DestX - Error) < LocatX) {if(DiffX < Error){ArrivalX = true;//X到达指定位置UWBCtrl.roll = 0;}else if(DestX < LocatX ) {UWBCtrl.roll = -((float)Fly_Speed) * 0.25f;//-Fly_Speed}else if(DestX > LocatX ) {UWBCtrl.roll = ((float)Fly_Speed) * 0.25f;//Fly_Speed}}//俯仰飞行控制数据if(!ArrivalY) {//if((DestY + Error) > LocatY && (DestY - Error) < LocatY) {if(DiffY < Error){ArrivalY = true;//Y到达指定位置UWBCtrl.pitch = 0;}else if(DestY < LocatY ) {UWBCtrl.pitch = -((float)Fly_Speed) * 0.25f;//-Fly_Speed;}else if(DestY > LocatY ) {UWBCtrl.pitch = ((float)Fly_Speed) * 0.25f;//Fly_Speed}}//XY到到达指定位置if(ArrivalX && ArrivalY) {Arrival_Flag = true;//到达目标位置//复位准备下次定点飞行}}//---------------------------------------------2022/5/22//只要有一个方向出现了偏移else if(DiffX > Deviation || DiffY > Deviation){if(++count > 10)//连续10次获取到的当前位置偏离目标点才重新执行自主飞行代码re_arrival_fly = true;}else{count = 1;//不连续重新置位1}//继续执行自主飞行if(re_arrival_fly){count = 1;Arrival_Flag = false;ArrivalX = false;ArrivalY = false;re_arrival_fly = false;}}else if(data[1] == 0x03) { //控制数据if(DestX != 0 && DestY != 0){//使无人机在有目标点的情况下可以使用键盘控制而不回到目标点FirstOrder = false;DestX = 0;DestY = 0;}UWBCtrl.thrust = (u16)data[2] << 8;                       //thrust :0~63356UWBCtrl.pitch  = ((float)data[3]-(float)0x80)*0.25f; //pitch:±9.5 ±19.2 ±31.7UWBCtrl.roll   = ((float)data[4]-(float)0x80)*0.25f;  //roll: ±9.5 ±19.2 ±31.7UWBCtrl.yaw    = ((float)data[5]-(float)0x80)*1.6f;       //yaw : ±203.2countfly = 1;//重新开始计数//（在接收到下一个指令前一直保持控制数，当不再接收到控制数据，就在一定时间后恢复定点）}else if(data[1] == 0x04) { //RGB，标志位数据UWBCmdProcess(data[2]);//位标志命令解析FlagData = data[2];Color_R = data[3];Color_G = data[4];Color_B = data[5];}UWBCtrl.trimPitch = 0;UWBCtrl.trimRoll = 0;setCommanderCtrlMode(3);//定点模式setCommanderFlightmode(0);//有头模式flightCtrldataCache(WIFI, UWBCtrl);//飞机控制数据发送到执行函数
}

上位机开发

最后一部分就是上位机的开发了，该部分涉及了UWB定位开启，UWB参数配置，还有灯光控制，目标点发送等等功能。
第一部分就是基于广州联网科技有限公司的UWB参数配置功能，基本没什么改动。

第二部分就开发出了
控制架次的选择：可以选择控制所有无人机或者控制单台无人机；
目标点设置：用于控制无人机的定点飞行；
键盘控制无人机：可以通过键盘的方向键发送控制指令从而控制无人机的俯仰和横滚；
编队路径设置：通过录入多组目标点，可以设置出一条航线，让无人机按照航线飞行；
编队队形：还有四个基础的控制队形，用于无人机集群的编队展示（视频中就实现了三台无人机的三角队形和直线队形）；
灯光控制：为了无人机编队表演的效果，我们在无人机上搭载了全彩LED，通过发送控制指令控制无人机的灯光颜色；

这部分是我同学开发的，为保护其知识产权，我就不擅自分享代码了。

至此，无人机的室内编队表演系统雏形就基本形成，当然还有许多值得优化的方面，比如UWB定位算法的优化，提高其定位速率、比如定点飞行算法的优化，添加PID控制算法，让无人机的定点飞行更加丝滑、以及无人机的防碰撞机制等等。。。

基于UWB的室内定位MiniFly无人机编队相关推荐

基于UWB的室内SDS_TWR测距算法优化和定位算法融合的研究
1.内容简介略257 2.内容说明 1.RSSI定位方法基于接收信号强度RSS(Receive Signal Strength)[57]方法通过三个及以上己知位置的锚节点来测量移动节点发射的信号场 ...
基于UWB的高精度定位
基于UWB的室内高精度定位系统 UWB定位简介超宽带(Ultra Wide-Band,UWB)是一种新型的无线通信技术,根据美国联邦通信委员会的规范,UWB的工作频带为3.1~10.6GHz,系统- ...
基于Wi-Fi的室内定位在美团总部的实践和应用（上）
室内定位技术的商业化必将带来一波创新高潮,尤其是在O2O领域,各种基于此技术的应用将出现在我们的面前.我们可以想象一些比较常见的应用场景,比如在大型商场里面借助室内导航快速找到目标商铺,商店根据用户的 ...
基于Wi-Fi的室内定位在美团总部的实践和应用
http://tech.meituan.com/mt-wifi-locate-practice-part1.html 室内定位技术的商业化必将带来一波创新高潮,尤其是在O2O领域,各种基于此技术的应用 ...
基于深度学习KNN的基于RSSI的室内定位系统仿真与实现
一.题目要求基于深度学习的基于RSSI的室内定位系统仿真与实现 1.利用一种深度学习方法,基于蓝牙RSSI信息,实现定位位置的估计与预测. 2.数据来源:互联网上有一种公开RSSI数据集. 3.图形 ...
基于RSSI的室内定位系统设计——使用PyhtonWeb开发
一.室内定位系统功能: 设计一种基于移动智能终端的室内定位系统,系统训练阶段,通过在用户随身携带的移动智能终端自动采集室内无线信号强度信息,数据经预处理后建立信号强度和位置的指纹库:定位阶段,用户移动 ...
基于WiFi的室内定位
摘要 GPS难以解决室内环境下的一些定位问题,大部分室内环境下都存在WiFi,因此利用WiFi进行定位无需额外部署硬件设备,是一个非常节省成本的方法.然而WiFi并不是专门为定位而设计的,传统的基于时 ...
计算机视觉室内定位的原理,基于计算机视觉的室内定位与导航系统
摘要: 室内定位与导航技术是当前热门研究技术,随着大型建筑物的日益增多,此项技术愈发重要.室内定位与导航与如今人们常用的通过GPS或北斗卫星导航的室外导航技术不同,由于墙壁的阻挡与折射,室外卫星信号会 ...
java wifi定位原理_基于wifi的室内定位方法深度分析.doc
大连理工大学本科毕业设计(论文) 基于WiFi信号的室内定位方法 Indoor localization method based on the WiFi signal 学院(系): 软件学院专 ...

基于UWB的室内定位MiniFly无人机编队