CN110221606B - 一种基于测距信号的距离变化率求解及机器人编队方法 - Google Patents

一种基于测距信号的距离变化率求解及机器人编队方法 Download PDF

Info

Publication number
CN110221606B
CN110221606B CN201910417509.9A CN201910417509A CN110221606B CN 110221606 B CN110221606 B CN 110221606B CN 201910417509 A CN201910417509 A CN 201910417509A CN 110221606 B CN110221606 B CN 110221606B
Authority
CN
China
Prior art keywords
distance
rate
change rate
solution
solving
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910417509.9A
Other languages
English (en)
Other versions
CN110221606A (zh
Inventor
张民
林云
郝慧鑫
梅劲松
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201910417509.9A priority Critical patent/CN110221606B/zh
Publication of CN110221606A publication Critical patent/CN110221606A/zh
Application granted granted Critical
Publication of CN110221606B publication Critical patent/CN110221606B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Measurement Of Optical Distance (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于测距信号的距离变化率求解及机器人编队方法，设计一种用于需要测量距离变化率但只能提供距离信息的工程应用场景之中。本发明设计了利用距离、距离求解值和距离变化率求解值得到距离变化率的方法，并对该方法的收敛性进行了分析。本发明还将求解方法运用到一个地面机器人编队问题的实例中以验证合理性。最后将本发明求解方法与已有的求解方法进行对比，同时加入信号扰动，说明本发明求解方法的优越性。

Description

一种基于测距信号的距离变化率求解及机器人编队方法
技术领域
本发明涉及一种基于测距信号的距离变化率求解及机器人编队方法，属于计量技术领域。
背景技术
随着近年来科技水平的不断提高，很多工程领域如机器人自动控制领域需要测量大量的距离、距离变化率、速度、加速度等信息，以此达到精确控制的目的。其中，针对距离变化率的测量是非常重要的。在当前的技术条件限制下，动态的距离变化率信号难以测量或测量值精度不高，并且存在易受噪声干扰的问题。相对而言，利用已经成熟的测距技术手段，如采用电波辐射源与返回点间的往返距离的电波相位迟后或直接与传播时间成正比进行测量，可以得到精度非常高的距离信息，以此求解距离变化率信息是可行的手段。这种求解方法具有良好的快速性、稳定性和精确度，既可以提高距离变化率信号的精度，又可以回避直接测量的难题，降低成本。对于距离变化率的求解方法，主要难点是保证求解方法的收敛性。
发明内容
本发明所要解决的技术问题是：提供一种基于测距信号的距离变化率求解及机器人编队方法，克服了实际工程应用中实际测量或解算的距离变化率信号精度差和易受噪声干扰的问题。
本发明为解决上述技术问题采用以下技术方案：
一种基于测距信号的距离变化率求解方法，包括如下步骤：
利用测距传感器获取距离信号，根据距离信号设计求解距离变化率的微分方程组，所述求解距离变化率的微分方程组为：
Figure BDA0002064901590000021
其中，s表示距离，
Figure BDA0002064901590000022
表示距离求解量，
Figure BDA0002064901590000023
表示距离求解量的导数，
Figure BDA0002064901590000024
表示距离变化率的求解值，
Figure BDA0002064901590000025
表示距离变化率的求解值的导数，a、b、c均为增益，sgn(·)为符号函数，
Figure BDA0002064901590000026
作为本发明的一种优选方案，距离变化率求解方法的收敛性分析方法为：
采用李雅普诺夫方法进行收敛性分析，所述李雅普诺夫方法的方程为：
L＝[|x1|sgn(x1),x1,x2]·J·[|x1|sgn(x1),x1,x2]T
其中，
Figure BDA0002064901590000027
s表示距离，
Figure BDA0002064901590000028
表示距离求解量，
Figure BDA0002064901590000029
表示距离变化率，
Figure BDA00020649015900000210
表示距离变化率的求解值，
Figure BDA00020649015900000211
a、b、c均为增益，d>0。
一种基于测距信号的机器人编队方法，包括如下步骤：
首先，利用测距传感器采集距离信号，将距离信号代入求解距离变化率的微分方程组，解算得到距离变化率；然后，将求解的距离变化率和测得的距离信号传入制导模块，由制导模块计算得到编队机器人的控制信号；最后将控制信号输入机器人模块，实现机器人编队。
本发明采用以上技术方案与现有技术相比，具有以下技术效果：
1、本发明提供了一种可用于实际工程中的基于测距信号的距离变化率求解方法。
2、本发明用这种求解方法得到的距离变化率比现有方法更精确、收敛速度更快。
3、本发明这种求解方法对信号噪声具有更好的抗干扰能力。
附图说明
图1是应用本方法求解距离变化率的实际应用模型。
图2是图1实施例中实际距离变化率与本发明求解的距离变化率对比。
图3是图1实施例中实际距离变化率、本发明求解方法以及现有求解方法对比。
图4是图1实施例基础上增加高斯扰动的实际应用模型。
图5是增加高斯噪声前后本发明求解的距离变化率对比。
具体实施方式
下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。
1、基于测距信号的距离变化率求解方法
根据已知的距离测量信息，设计对距离变化率求解的微分方程组：
Figure BDA0002064901590000031
其中，a、b、c为增益，sgn(·)为符号函数，
Figure BDA0002064901590000032
s表示距离，
Figure BDA0002064901590000033
表示距离求解量，
Figure BDA0002064901590000034
表示距离求解量的导数，
Figure BDA0002064901590000035
表示距离变化率的求解值，
Figure BDA0002064901590000036
表示距离变化率的求解量的导数。
2、对提出的距离变化率求解方法采用李雅普诺夫方法进行收敛性分析
定义变量
Figure BDA0002064901590000037
由此得到
Figure BDA0002064901590000038
定义函数g(t,x1,x2)，其满足|g(t,x1,x2)|≤μ1|x1|+μ2|x2|，μ1和μ2表示扰动，且满足μ1>0，μ2>0。则式(1)可改写为
Figure BDA0002064901590000039
定义变量η＝[|x1|sgn(x1),x1,x2]T，提出李雅普诺夫函数
L＝ηTJη (3)
其中
Figure BDA0002064901590000041
且d>0。对式(3)求导，得到
Figure BDA0002064901590000042
其中
Figure BDA0002064901590000043
Figure BDA0002064901590000044
因为|g(t,x1,x2)|≤μ1|x1|+μ2|x2|，进而得到
Figure BDA0002064901590000045
Figure BDA0002064901590000046
将式(5)，(6)代入(4)，得到
Figure BDA0002064901590000047
其中
Figure BDA0002064901590000048
因为Z1+Z2-Z3-Z4是正定的，所以
Figure BDA0002064901590000049
又因为ηT(Z1-Z3)η≥λmin(Z1-Z3)||η||2，所以
Figure BDA00020649015900000410
λmin(·)表示正定对称矩阵的最小特征值；又因为L≤λmax(J)||η||2，所以
Figure BDA00020649015900000411
λmax(·)表示正定对称矩阵的最大特征值。
综合上述分析，推出
Figure BDA0002064901590000051
对式(7)两边积分，积分区间为[0,t]，得到
Figure BDA0002064901590000052
由式(8)可知，在一定时间内，L趋于0。综合分析式(3)、式(8)，根据李雅普诺夫稳定性判别定理可知，当L>0且
Figure BDA0002064901590000053
时，式(2)对应的系统处于平衡状态，此时计算得到参数范围0<a<2.5，
Figure BDA0002064901590000054
c>0，d>0。当L＝0，对应平衡点(x1,x2)＝(0,0)。由此可知式(1)对应的系统也是收敛的，且对应的平衡点
Figure BDA0002064901590000055
综合1、2中的介绍，本发明中基于距离的距离变化率求解方法是稳定有效的。将该求解方法应用于一个地面机器人编队问题实例中，其MATLAB模型如图1所示。在该实际工程应用场景中，只采集距离信号，将该信号传入解算模块，求得编队制导需要的距离变化率信号；然后，同时将求解的距离变化率和测得的距离信号传入制导模块，由制导模块计算得到编队机器人的控制信号；最后将控制信号输入机器人模块，实现机器人编队。
在该实际应用场景中，一共使用了4台装有测距传感器的可移动机器人平台，每台机器人都配有型号相同的微型计算机以及信号接收装置。4台机器人的初始位置可以在信号接收装置以及距离传感器的有效范围内任意选取。最终编队目标是经过一段时间后，各机器人均匀分布在以某个固定目标为圆心，一定长度为半径的圆上，各机器人之间距离相等，并作圆周运动，在编队过程中，各机器人之间不会相撞。
该过程中本发明的求解算法结果，如图2所示。其中，实际距离变化率由测得的距离信号通过仿真工具中的微分模块得到。
在图1实施例基础上，与现有公开的一种具有代表性的方法(精密测距模拟器距离变化率参数的校准方法，《计量与测试技术》2014年第41卷第5期)进行了对比，结果如图3所示，可以看出本发明中提供的方法较之前出现的方法，其收敛的快速性以及与实际距离变化率对比的准确性都有很大提升。
在图1实例基础上，加入高斯噪声作为扰动，模型结构如图4所示。其仿真结果如图5所示，可以看出本发明中提供的方法对噪声具有抗干扰能力。
以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (3)

1.一种基于测距信号的距离变化率求解方法，其特征在于，包括如下步骤：
利用测距传感器获取距离信号，根据距离信号设计求解距离变化率的微分方程组，所述求解距离变化率的微分方程组为：
Figure FDA0003105585100000011
其中，s表示距离，
Figure FDA0003105585100000012
表示距离求解量，
Figure FDA0003105585100000013
表示距离求解量的导数，
Figure FDA0003105585100000014
表示距离变化率的求解值，
Figure FDA0003105585100000015
表示距离变化率的求解值的导数，a、b、c均为增益，sgn(·)为符号函数，
Figure FDA0003105585100000016
2.根据权利要求1所述基于测距信号的距离变化率求解方法，其特征在于，距离变化率求解方法的收敛性分析方法为：
采用李雅普诺夫方法进行收敛性分析，所述李雅普诺夫方法的方程为：
L＝[|x1|sgn(x1),x1,x2]·J·[|x1|sgn(x1),x1,x2]T
其中，
Figure FDA0003105585100000017
s表示距离，
Figure FDA0003105585100000018
表示距离求解量，
Figure FDA0003105585100000019
表示距离变化率，
Figure FDA00031055851000000110
表示距离变化率的求解值，
Figure FDA00031055851000000111
a、b、c均为增益，d＞0。
3.一种基于测距信号的机器人编队方法，其特征在于，包括如下步骤：
首先，利用测距传感器采集距离信号，将距离信号代入求解距离变化率的微分方程组，解算得到距离变化率；然后，将求解的距离变化率和测得的距离信号传入制导模块，由制导模块计算得到编队机器人的控制信号；最后将控制信号输入机器人模块，实现机器人编队；
所述求解距离变化率的微分方程组为：
Figure FDA00031055851000000112
其中，s表示距离，
Figure FDA0003105585100000021
表示距离求解量，
Figure FDA0003105585100000022
表示距离求解量的导数，
Figure FDA0003105585100000023
表示距离变化率的求解值，
Figure FDA0003105585100000024
表示距离变化率的求解值的导数，a、b、c均为增益，sgn(·)为符号函数，
Figure FDA0003105585100000025
CN201910417509.9A 2019-05-20 2019-05-20 一种基于测距信号的距离变化率求解及机器人编队方法 Active CN110221606B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910417509.9A CN110221606B (zh) 2019-05-20 2019-05-20 一种基于测距信号的距离变化率求解及机器人编队方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910417509.9A CN110221606B (zh) 2019-05-20 2019-05-20 一种基于测距信号的距离变化率求解及机器人编队方法

Publications (2)

Publication Number Publication Date
CN110221606A CN110221606A (zh) 2019-09-10
CN110221606B true CN110221606B (zh) 2021-08-17

Family

ID=67821404

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910417509.9A Active CN110221606B (zh) 2019-05-20 2019-05-20 一种基于测距信号的距离变化率求解及机器人编队方法

Country Status (1)

Country Link
CN (1) CN110221606B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2809468B2 (ja) * 1989-03-06 1998-10-08 レイセオン・カンパニー スペクトラム拡散信号検出装置
CN104266546A (zh) * 2014-09-22 2015-01-07 哈尔滨工业大学 一种基于视线的有限时间收敛主动防御制导控制方法
CN104943694A (zh) * 2014-03-28 2015-09-30 通用汽车环球科技运作有限责任公司 用于确定和补偿传感器的未对准的系统和方法
CN107422748A (zh) * 2017-06-29 2017-12-01 南京航空航天大学 一种固定翼无人机编队制导装置及协同跟踪制导方法
CN107526073A (zh) * 2017-08-22 2017-12-29 哈尔滨工程大学 一种运动多站无源时差频差联合定位方法
CN108227736A (zh) * 2017-12-18 2018-06-29 南京航空航天大学 一种基于测距信号的固定翼无人机编队跟踪制导方法
CN108646554A (zh) * 2018-05-04 2018-10-12 北京航空航天大学 一种基于指定性能的飞行器快速抗干扰纵向制导方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2809468B2 (ja) * 1989-03-06 1998-10-08 レイセオン・カンパニー スペクトラム拡散信号検出装置
CN104943694A (zh) * 2014-03-28 2015-09-30 通用汽车环球科技运作有限责任公司 用于确定和补偿传感器的未对准的系统和方法
CN104266546A (zh) * 2014-09-22 2015-01-07 哈尔滨工业大学 一种基于视线的有限时间收敛主动防御制导控制方法
CN107422748A (zh) * 2017-06-29 2017-12-01 南京航空航天大学 一种固定翼无人机编队制导装置及协同跟踪制导方法
CN107526073A (zh) * 2017-08-22 2017-12-29 哈尔滨工程大学 一种运动多站无源时差频差联合定位方法
CN108227736A (zh) * 2017-12-18 2018-06-29 南京航空航天大学 一种基于测距信号的固定翼无人机编队跟踪制导方法
CN108646554A (zh) * 2018-05-04 2018-10-12 北京航空航天大学 一种基于指定性能的飞行器快速抗干扰纵向制导方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于激光测距和惯导的非合作目标远程交会相对导航算法研究;杨东春 等;《上海航天》;20160430;第33卷(第4期);全文 *
精密测距模拟器距离变化率参数的校准方法;程翊昕 等;《计量与测试技术》;20140531;第41卷(第5期);全文 *

Also Published As

Publication number Publication date
CN110221606A (zh) 2019-09-10

Similar Documents

Publication Publication Date Title
CN103197279B (zh) 一种移动目标协同定位系统的定位方法
CN105043348A (zh) 基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法
CN111948602A (zh) 基于改进Taylor级数的二维UWB室内定位方法
KR101914550B1 (ko) 레이더의 표적 위치 추적 방법
CN103017771B (zh) 一种静止传感器平台的多目标联合分配与跟踪方法
CN110988894B (zh) 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法
CN103954179A (zh) 捷联红外导引头隔离度寄生回路评估系统
CN109615860A (zh) 一种基于非参数贝叶斯框架的信号交叉口状态估计方法
CN109490855A (zh) 一种车载雷达标定方法、装置及车辆
CN105353789A (zh) 连续振动信号时间历程复现控制方法
CN106354016B (zh) 一种金属管夯实高度的控制方法
CN104159297A (zh) 一种基于聚类分析的无线传感器网络多边定位算法
CN110941285A (zh) 一种基于双ip核的无人机飞行控制系统
CN110221606B (zh) 一种基于测距信号的距离变化率求解及机器人编队方法
CN114739397A (zh) 矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法
Wang et al. Robust filter method for SINS/DVL/USBL tight integrated navigation system
CN111624549B (zh) 非共视条件下无源滤波跟踪方法
CN103092815A (zh) 校准装置及其对监测装置中的传递函数进行校准的方法
CN104268597A (zh) 基于ahcif的集中式测量值扩维融合方法
CN108919313A (zh) 利用最优数值导数的gnss多普勒观测值生成方法
CN114915931A (zh) 一种基于rssi技术的室内定位方法
CN113341385B (zh) 机载平台协同综合传感器系统马尔科夫链误差传递模型
CN104635246A (zh) 卫星导航信号的动态范围检测系统及检测方法
CN103218509A (zh) 一种基于硬件的复合抗干扰数据关联方法
CN112489075A (zh) 基于特征函数的序惯式多传感器融合滤波方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant