CN111061269A - 基于uwb定位的水上智能救援机器系统及其方法 - Google Patents

Abstract

本发明公开了水上救援技术领域的基于UWB定位的水上智能救援机器系统，包括活动标签，定位基站、软件平台和救援机器人，活动标签能够发射UWB信号；定位基站能够接收从活动标签发送过来的UWB信号并经过自适应算法分析出标签位置；软件平台获取、分析并传输信息给用户和其他相关信息系统，并指派救援机器人进行救援；救援机器人包括MCU控制模块、电源模块、动力装置，MCU控制模块采用stm32f407单片机作为主控MCU，动力装置包括直流有刷电机、螺旋桨和驱动电路，螺旋桨设置有两组分别安装在救援机器人的两侧，直流有刷电机驱动螺旋桨转动，实现救援机器人的转动，本发明替代人工来进行游泳池水上救援，使得救援速度大幅提高，提高了救援的可靠性。

Description

基于UWB定位的水上智能救援机器系统及其方法

技术领域

本发明涉及水上救援技术领域，具体为基于UWB定位的水上智能救援机器系统。选取合适的定位算法,将测量参数代入定位算法中,求出待测目标的位置估计坐标。

背景技术

目前海边、游泳池等场所大多靠救生人员携带救生装备(如救生圈、救生衣等)对求救人员进行救援，都建立在人为的基础上，救援速度慢、效率低，没法对多个待救目标进行施救，同时在救援的过程中也会对救生人员带来一定的危险，导致悲剧的发生。

为了保障人们的生命安全，可以让机器人代替人做一些繁重、危险的工作。机器人已经能够使用工具,能看、能听、能说，并且开始能进行一些决策和思考等智能行为，其应用也从传统的加工制造业逐渐扩展到海军、海洋检测、宇宙探索等领域，并开始进入家庭和服务行业。作为一种先进的机电一体化产品，机器人的技术发展与自动控制技术的发展息息相关。人们期望机器人在许多人类所不能及的区域代替人类劳动完成更复杂的任务。例如在军事侦察、扫雷排险、防止核化污染等危险与恶劣环境以及民用中的物料搬运上具有广阔的应用前景。机器人所面临的是未知的、不可预测的非人为构造的环境,机器人所完成的任务也越来越复杂。

目前，国内外的救援机器人可以说是让人眼花缭乱，但是现在的救援机器人都是针对陆地侦察、救援的，基本没有对水上救援机器人进行研究的，而现存的水上救援基本上都是通过遥控来进行救援的，虽然速度上有所提高，但这种救援机器人在很大程度上还是要依靠人力，可以说水上自动救援领域存在一个比较大的空白。

目前的UWB定位技术主要运用在室内人员定位，隧道人员定位等，其定位精度可以达到5到10厘米，完全可以达到救援精度，其在运用在游泳池来进行人员定位，来进行水上救援的系统，目前也没有应用。

传统的溺水救援以低效、高代价的人工救援为主，目前市场上并没有水上自动救援智能机器人。基于此，本发明设计了基于UWB定位的水上智能救援机器系统，以解决上述问题。

发明内容

本发明的目的在于提供基于UWB定位的水上智能救援机器系统及其方法，以解决上述背景技术中提出“传统的溺水救援以低效、高代价的人工救援为主，目前市场上并没有水上自动救援智能机器”的问题。

为实现上述目的，本发明提供如下技术方案：基于UWB定位的水上智能救援机器系统，包括活动标签，定位基站、软件平台和救援机器人，所述活动标签能够发射UWB信号；所述定位基站能够接收从活动标签发送过来的UWB信号,再根据接收到的UWB信号计算定位标签的坐标；

选取合适的定位算法,将测量参数代入定位算法中,求出待测目标的位置估计坐标。结合UWB室内三维定位特点,将常用的测距方式二维模型扩展为三维模型,并分析优缺点后,决定选用基于接收信号时间差法(TDOA)作为测距方式。

所述软件平台获取、分析并传输信息给用户和其他相关信息系统，并指派所述救援机器人进行救援；所述救援机器人包括MCU控制模块、电源模块、动力装置，所述MCU控制模块采用stm32f407单片机作为主控MCU，所述动力装置包括直流有刷电机、螺旋桨和驱动电路，所述螺旋桨设置有两组分别安装在所述救援机器人的两侧，所述直流有刷电机驱动螺旋桨转动，所述驱动电路调节所述直流有刷电机的转速。

优选的，所述电源模块包括蓄电池、整流桥电路和降压电路模块，所述蓄电池通过整流桥电路连接市电进行充电，所述蓄电池选用24V直流稳压蓄电池，所述蓄电池通过所述降压电路模块降压后，给所述MCU控制模块供电。

优选的，所述降压电路模块采用贴片LM2596降压模块。

优选的，所述动力装置包括直流有刷电机、螺旋桨和驱动电路，所述螺旋桨设置有两组分别安装在所述救援机器人的两侧，所述直流有刷电机驱动螺旋桨转动。

优选的，所述MCU通过驱动电路控制所述直流有刷电机的转速，使两组所述螺旋桨产生差速，实现救援机器人的转动。

优选的，所述活动标签刷新率从0.002Hz-33Hz根据环境自动分配。

优选的，所述活动标签内置振动传感器和纽扣电池。

优选的，所述定位基站通过到达时间差测量技术(TDOA)，对水中环境误差进行分析并建模的基础上,针对信号传播过程中受到非视距(NLOS)误差的影响,采用卡尔曼滤波算法对静止和运动状态的距离差测量值进行数据重构,再将重构后的距离差代入Chan算法,得出待测目标的三维位置估计,又引进位置差分方法,修正定位标签的位置坐标，来最终确定活动标签的位置，并将数据传输至网络控制器及软件平台，所述软件平台接受信号并指派所述救援机器人进行救援。

优选的，所述活动标签匹配独立的ID号。

基于UWB定位的水上智能救援方法，方法包括以下步骤：

步骤1：实时监测人员的位置，软件平台实时显示人员位置；

步骤2：使用者按下救援按钮，活动标签发出信号；

步骤3：定位基站接收信号并估算出待救援者位置，并将位置数据上传给软件平台，传递救援信号；

步骤4：软件平台发送信号给救援机器人，派使救援机器人救人；

步骤5：救援机器人利用螺旋桨的推动实现前进，利用螺旋桨的差速实现转动，救援机器人迅速到达目标位置，投放救援设备。

与现有技术相比，本发明的有益效果是：本发明是基于UWB定位的水上智能救援机器系统，完全弥补了基于水上人员定位的自动救援机器人领域的空白，并基于水中环境和人群间隔对定位算法进行调整，使在多种复杂环境共同存在时,误差稳定在20cm左右。该作品替代人工来进行游泳池水上救援，使得救援速度大幅提高，也可以安装多个机器人，同时对多个待救目标进行救援，大大提高了救援效率，提高了救援的可靠性。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明三维定位布置示意图；

图2为本发明整流桥电路结构示意图；

图3为本发明LM2596电路图；

图4为本发明驱动电路图图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本实施例的一个具体应用为：

请参阅图1-4，本发明提供一种技术方案：基于UWB定位的水上智能救援机器系统，包括活动标签，定位基站、软件平台和救援机器人，活动标签能够发射UWB信号；定位基站能够接收并估算从活动标签发送过来的UWB信号；软件平台获取、分析并传输信息给用户和其他相关信息系统，并指派救援机器人进行救援；救援机器人包括MCU控制模块、电源模块、动力装置，MCU控制模块采用stm32f407单片机作为主控MCU，STM32F407提供了工作频率为168MHz的Cortex^TM-M4内核(具有浮点单元)的性能，在168MHz频率下，从Flash存储器执行时，STM32F407/417能够提供210DMIPS/566CoreMark性能，并且利用意法半导体的ART加速器实现了FLASH零等待状态，DSP指令和浮点单元扩大了产品的应用范围，动力装置包括直流有刷电机、螺旋桨和驱动电路，螺旋桨设置有两组分别安装在救援机器人的两侧，直流有刷电机驱动螺旋桨转动，驱动电路调节直流有刷电机的转速。

电源模块包括蓄电池、整流桥电路和降压电路模块，蓄电池通过整流桥电路连接市电进行充电，蓄电池选用24V直流稳压蓄电池，蓄电池通过降压电路模块降压后，给MCU控制模块供电，将交流电源过整流桥和滤波电路，转化成24V直流稳压电源，作为机器人电机的驱动电源，保证救援机器人的长期工作状态。

降压电路模块采用贴片LM2596降压模块，LM2596开关电压调节器是降压型电源管理单片机集成电路，能够输出3A的驱动电流，并且能够通过电位器输出可调的电压，同时具有很好的线性和负载调节特性。该器件内部集成频率补偿和固定频率发生器,开关频率为150KHz，可以使用通用的标准电感，极大地简化了开关电源电路的设计。

动力装置包括直流有刷电机、螺旋桨和驱动电路，螺旋桨设置有两组分别安装在救援机器人的两侧，直流有刷电机驱动螺旋桨转动，驱动电路板控制驱动具有电流大，功率大的特点。

MCU模块通过驱动电路控制直流有刷电机的转速，使两组螺旋桨产生差速，实现救援机器人的转动。

活动标签刷新率从0.002Hz-33Hz根据环境自动分配。

活动标签内置振动传感器和纽扣电池，可支撑1年的供电。

为了减少定位误差，定位基站基于接收信号时间差法(TDOA)，采用卡尔曼滤波算法对静止和运动状态的距离差测量值进行数据重构,再将重构后的距离差代入Chan算法,得出待测目标的三维位置估计后，引进位置差分方法,修正定位标签的位置坐标，最终确定活动标签的位置，并将数据传输至网络控制器及软件平台，软件平台接受信号并指派救援机器人进行救援,且活动标签匹配独立的ID号。

选取合适的定位算法,将测量参数代入定位算法中,求出待测目标的位置估计坐标。结合UWB室内三维定位特点,将常用的测距方式二维模型扩展为三维模型,并分析优缺点后,决定选用基于接收信号时间差法(TDOA)作为测距方式。在分别对基于TDOA的Fang算法和Chan算法进行推导和仿真分析后,发现Fang算法对基站数量有限制,定位结果更分散,所以采用Chan算法为本设备的主要算法。对环境误差进行分析并建模,针对信号传播过程中受到非视距(NLOS)误差的影响,采用卡尔曼滤波算法对静止和运动状态的距离差测量值进行数据重构,再将重构后的距离差代入Chan算法,得出待测目标的三维位置估计,定位精度有所提高,又引进位置差分方法,修正定位标签的位置坐标。对上述算法建立数学模型,并在实验室搭建实验环境采集数据,用MATLAB对定位结果进行仿真分析,用C++编程实现待测目标在上位机界面实时定位跟踪,结果显示信号到达基站的时间差数据在Chan算法计算后得到初步位置坐标的均方根误差,经过卡尔曼滤波后减小了37％,加入差分方法后又减小了69％。在复杂环境下进行大量实验,得出在单一的复杂环境下,误差不超过12cm,在多种复杂环境共同存在时,误差稳定在20cm左右。

采用基于TDOA的Chan算法，在用户按下救援按钮后，通过基站接收到信号的时间测量值，建立以待测目标坐标为未知数的距离差方程，在误差服从高斯分布的情况下，通过求解方程组得出的位置精确度较高，Chan算法具体流程为：第一步各基站接收待测目标的坐标值，第二步为消除时延，具体方法为在使用估计距离差代替精确距离差后代入噪声误差适量的协方差矩阵并使用加权最小二乘计算待测目标的位置估计值，第三步为减小噪声干扰下变量间相互干扰的影响，具体方法为利用到达时间差TODA的协方差矩阵代替噪声误差矢量的协方差矩阵进而修改定位标签估计值，后求出利用环境误差的协方差矩阵再次利用加权最小二乘法(WLS，以后用WLS表示)得出最终目标估计值。

假设在空间中分布了M个基站(M>＝4)，待测目标坐标为BQ(x,y,z)，所有基站位置已知，基站坐标为JZ_i(x_i,y_i,z_i)，(i＝1,2,3,4)，假设基站i到待测目标BQ的精确距离为r_i ⁰，时延时间为t_i，则：

以基站1作为基准，待测目标到基站1的精准距离为r₁ ⁰，基站1接收到信号的时延值为t₁，则待测目标到两基站的精确距离差为：

待测目标到两基站的估计距离差为：

在没有时延误差的情况下，下式成立：

则得到：

由式(1-1)和式(1-5)得：

其中x_i1＝x_i-x₁，y_i1＝y_i-y₁，z_i1＝z_i-z₁。

实际定位中，必然存在时延误差，用式(1-3)中的r_i1代替式(1-6)中的精确距离差

再由

得到：

将上式写成矩阵形式：

其中：

噪声误差矢量

表示Schur乘积，其中Δt＝[Δt₂₁ Δt₃₁... Δt_n1]^T，

在实际应用中，cΔt＜＜r_i ⁰，因此噪声误差矢量方程中

可以省略，

的协方差矩阵为：

其中Q为到达时间差TDOA的协方差矩阵，

为其他基站与基准基站的到达时间差测量值方差，使用加权最小二乘(WLS)计算，得出待测目标的坐标估计值为：

将Q代替上式中φ，则定位标签估计值可以近似表示为：

由式(1-11)和(1-13)得定位标签估计值：

则B矩阵的估计值为：

将矩阵B’代入

的协方差方程中，

再带入式(1-12)，再重复使用加权最小二乘法(WLS)计算，求出的待测目标BQ位置估计值更准确。前面的运算没考虑z_a中的各个元素相互关系，假设r₁ ⁰,x,y,z元素相互独立，但在实际中，r₁ ⁰与待测目标的位置坐标x,y,z是相关的，计算出z_a的协方差方程，才能得到定位标签更精确的位置估计，在有噪音干扰的环境下，r_i,G_a,h的表达式如下：

由式(1-8)得出误差矢量：

令

通过式(1-12)和(1-17)得：

室内定位环境复杂，测量值存在较大误差，z_a表示为：

z_a(1)＝r₁ ⁰+e₁，z_a(2)＝x+e₂，z_a(3)＝y+e₃，z_a(4)＝z+e₄ (1-19)

e₁,e₂,e₃,e₄为z_a受环境影响的误差矢量，由关系式r²＝x²+y²+z²可得：

复杂环境中的误差矢量

其中：

误差的协方差方程为：

再用一次加权最小二乘法(WLS)得出z_a的估计值：

待测目标的最终值为：

待测目标最终位置的正负号可以参照式(1-13)的

的值选取。

进行上述过程，得到定位标签在三维空间的位置坐标。此方法只要求基站时钟完全同步，降低了硬件成本，且大幅度提高了定位精度，适合于室内泳馆三维定位。

另外，本申请还涉及一种基于UWB定位的水上智能救援方法，方法包括以下步骤：

步骤1：实时监测游泳人员的位置，软件平台实时显示游泳人员位置；

步骤2：使用者(即游泳人员)按下救援按钮，佩戴在使用者身上的活动标签发出信号；

步骤3：定位基站接收信号并估算出待救援者位置，并将位置数据上传给软件平台，传递救援信号；

步骤4：软件平台发送信号给救援机器人，派使救援机器人救人；

步骤5：救援机器人利用螺旋桨的推动实现前进，利用螺旋桨的差速实现转动，救援机器人迅速到达目标位置，投放救援设备。

本申请中应用了UWB技术，使UWB技术应用于室内定位，具有以下特点：第一，系统容量大，香农公式给出C＝Blog2(1+S/N)可以看出，带宽增加使信道容量的提高远远大于信号功率上升所带来的效应，这一点也正是提出超宽带技术的理论机理，超宽带无线电系统用户数量大大高于3G系统。

其二，具有数据传输速度快的特点，UWB系统使用上吉赫兹的超宽频带，根据香农信道容量公式，即使把发送信号功率密度控制得很低，也可以实现高的信息速率。一般情况下，其最大数据传输速度可以达到几百兆比特每秒到吉比特每秒。

第三，多径分辨能力强，UWB由于其极高的工作频率和极低的占空比而具有很高的分辨率，窄脉冲的多径信号在时间上不易重叠，很容易分离出多径分量，所以能充分利用发射信号的能量。实验表明，对常规无线电信号多径衰落深达10～30dB的多径环境，UWB信号的衰落最多不到5dB。

第四、隐蔽性好，因为UWB的频谱非常宽，能量密度非常低，因此信息传输安全性高。另一方面，由于能量密度低，UWB设备对于其他设备的干扰就非常低。

第五、定位精确，冲激脉冲具有很高的定位精度，采用超宽带无线电通信，可在室内和地下进行精确定位，精度最高可达2厘米，一般精度在15厘米内。而GPS定位系统只能工作在GPS定位卫星的可视范围之内。与GPS提供绝对地理位置不同，超短脉冲定位器可以给出相对位置，其定位精度可达厘米级。

第六、抗干扰能力强，UWB扩频处理增益主要取决于脉冲的占空比和发送每个比特所用的脉冲数。UWB的占空比一般为0.01～0.001，具有比其他扩频系统高得多的处理增益，抗干扰能力强。一般来说，UWB抗干扰处理增益在50dB以上。

第七、低功耗，UWB无线通信系统接收机没有本振、功放、锁相环(PLL)、压控振荡器(VCO)、混频器等，因而结构简单，设备成本将很低。由于UWB信号无需载波，而是使用间歇的脉冲来发送数据，脉冲持续时间很短，一般在0.20～1.5ns之间，有很低的占空因数，所以它只需要很低的电源功率。一般UWB系统只需要50～70mW的电源，是蓝牙技术的十分之一。

本发明是一种基于UWB定位的水上智能救援机器系统，完全弥补了基于水上人员定位的自动救援机器人领域的空白，该作品替代人工来进行游泳池水上救援，使得救援速度大幅提高，也可以安装多个机器人，同时对多个待救目标进行救援，大大提高了救援效率，提高了救援的可靠性。

在本说明书的描述中，参考术语“一个实施例”、“示例”、“具体示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

Claims (10)

1.基于UWB定位的水上智能救援机器系统，其特征在于：包括活动标签，定位基站、软件平台和救援机器人，所述活动标签能够发射UWB信号；所述定位基站能够接收从活动标签发送过来的UWB信号并自适应处理得出标签坐标；所述软件平台获取、分析并传输信息给用户和其他相关信息系统，并指派所述救援机器人进行救援；所述救援机器人包括MCU控制模块、电源模块、动力装置，所述MCU控制模块采用stm32f407单片机作为主控MCU，所述动力装置包括直流有刷电机、螺旋桨和驱动电路，所述螺旋桨设置有两组分别安装在所述救援机器人的两侧，所述直流有刷电机驱动螺旋桨转动，所述驱动电路调节所述直流有刷电机的转速。

2.根据权利要求1所述的基于UWB定位的水上智能救援机器系统，其特征在于：所述电源模块包括蓄电池、整流桥电路和降压电路模块，所述蓄电池通过整流桥电路连接市电进行充电，所述蓄电池选用24V直流稳压蓄电池，所述蓄电池通过所述降压电路模块降压后，给所述MCU控制模块供电。

3.根据权利要求2所述的基于UWB定位的水上智能救援机器系统，其特征在于：所述降压电路模块采用贴片LM2596降压模块。

4.根据权利要求1所述的基于UWB定位的水上智能救援机器系统，其特征在于：所述动力装置包括直流有刷电机、螺旋桨和驱动电路，所述螺旋桨设置有两组分别安装在所述救援机器人的两侧，所述直流有刷电机驱动螺旋桨转动。

5.根据权利要求4所述的基于UWB定位的水上智能救援机器系统，其特征在于：所述MCU模块通过驱动电路控制所述直流有刷电机的转速，使两组所述螺旋桨产生差速，实现救援机器人的转动。

6.根据权利要求1所述的基于UWB定位的水上智能救援机器系统，其特征在于：所述活动标签刷新率从0.002Hz-33Hz根据环境自动分配。

7.根据权利要求4所述的基于UWB定位的水上智能救援机器系统，其特征在于：所述活动标签内置振动传感器和纽扣电池。

8.根据权利要求4所述的基于UWB定位的水上智能救援机器系统，其特征在于：所述定位基站通过到达时间差测量技术，将测量参数代入定位算法中,求出待测目标的位置估计坐标，来确定活动标签的位置，并将数据传输至网络控制器及软件平台，所述软件平台接受信号并指派所述救援机器人进行救援。

9.根据权利要求4所述的基于UWB定位的水上智能救援机器系统，其特征在于：所述活动标签匹配独立的ID号。

10.基于UWB定位的水上智能救援方法，其特征在于，

方法包括以下步骤：

步骤1：实时监测人员的位置，软件平台实时显示人员位置；

步骤2：使用者按下救援按钮，活动标签发出信号；

步骤3：定位基站接收信号并估算出待救援者位置，并将位置数据上传给软件平台，传递救援信号；

步骤4：软件平台发送信号给救援机器人，派使救援机器人救人；

步骤5：救援机器人利用螺旋桨的推动实现前进，利用螺旋桨的差速实现转动，救援机器人迅速到达目标位置，投放救援设备。

Images