CN111536967B - 一种基于ekf的多传感器融合温室巡检机器人跟踪方法 - Google Patents

一种基于ekf的多传感器融合温室巡检机器人跟踪方法 Download PDF

Info

Publication number
CN111536967B
CN111536967B CN202010274834.7A CN202010274834A CN111536967B CN 111536967 B CN111536967 B CN 111536967B CN 202010274834 A CN202010274834 A CN 202010274834A CN 111536967 B CN111536967 B CN 111536967B
Authority
CN
China
Prior art keywords
value
robot
matrix
observation
model
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
CN202010274834.7A
Other languages
English (en)
Other versions
CN111536967A (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.)
Jiangsu University
Original Assignee
Jiangsu University
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 Jiangsu University filed Critical Jiangsu University
Priority to CN202010274834.7A priority Critical patent/CN111536967B/zh
Publication of CN111536967A publication Critical patent/CN111536967A/zh
Application granted granted Critical
Publication of CN111536967B publication Critical patent/CN111536967B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Theoretical Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于EKF的多传感器融合温室巡检机器人跟踪方法。本发明首先通过UWB测距模块基于DS‑TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息，同时通过机器人搭载的MPU6050实时获取机器人的加速度和航向角。基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值，利用Taylor算法在观测点进行迭代修正，将修正后的坐标观测值，实时测量的机器人加速度和航向角同时传入CTRV运动模型，利用扩展的卡尔曼滤波对传感器数据进行融合处理，将扩展的卡尔曼滤波(EKF)结果作为机器人当前位置输出，得到一个多传感器融合的温室巡检机器人跟踪方法。本发明能够实现对温室巡检机器人在室内环境下的精确定位，减少温室管理的成本。

Description

一种基于EKF的多传感器融合温室巡检机器人跟踪方法
技术领域
本发明涉及一种基于EKF(扩展的卡尔曼滤波)的多传感器融合温室巡检机器人跟踪方法，可应用在室内机器人精确定位、温室巡检机器人轨迹跟踪等场景。
背景技术
随着温室大棚的逐年增加，温室的无人化管理已经成为了温室智能化、机械化发展的必然趋势，传统的温室由人来管理，而人的进出使温室很难和外界环境隔开，人容易将病虫害带入温室，且温室的管理琐碎而繁杂，会耗费人大量的精力，所以无人化管理几乎是温室发展的必然方向。温室巡检机器人作为温室无人管理系统重要的组成成分，是未来实现无人化农业，精准农业关键。温室巡检机器人是集温室植株信息收集，环境感知，管理作业等功能于一体的综合系统，能够大大减少温室管理人工成本，提高温室管理智能化程度。由于温室巡检机器人需要在温室中对植株进行监测作业，如何准确到达指定植株，如何分辨温室机器人采集的传感器信息是属于哪一株植株的，想要解决上述问题，就必须要获取机器人精确的位置信息，因此，在室内情况下的目标精确定位成为温室巡检机器人自主控制领域乃至室内机器人控制领域的核心问题之一。
随着温室精细农业的迅猛发展，众多学者开始了温室内的定位研究，WIDODO等在温室中设计了基于声波的定位系统自校准方法；LUO等利用RSSI测距技术进行了温室内的定位研究以及王俊等利用BP算法进行温室内的定位研究。但上述方法为提高精度,就必须安装大量的辅助节点，但增大了系统建设的成本和能耗。在测量误差和能源消耗不可避免的情况下,减小测量误差和控制能耗显得尤为重要。本专利提出使用UWB室内定位技术和惯导融合定位的方法对温室巡检机器人进行室内精确定位。UWB在众多室内定位技术中脱颖而出，具有精度高、发射功率小、辐射小、成本低、网络节点布置方便的优点，不仅可以作为无线通信传感器网络使用，同时可以作为定位系统使用，适合在温室等室内环境中使用，但单一的UWB室内定位的精度会受传感器偏差、灵敏度误差、非视距传播影响，本专利提出一种基于EKF的多传感器融合温室巡检机器人跟踪方法，提高了定位精度，实现了精确的目标跟踪。
发明内容
发明目的：随着温室大棚的逐年增加，温室的无人化管理已经成为了温室智能化、机械化发展的必然趋势，而室内精准定位是实现对单一植株的精准监控和作业的基本需求，但是传统的无线传感器网络往往是固定的静态节点，需要布置大量传感器来对整个温室进行监控，而采用机器人搭载传感器的方案则需要面对更加严苛的动态定位精度要求。本发明提出一种基于EKF的多传感器融合温室巡检机器人跟踪方法，基于EKF利用UWB测距技术与惯导进行融合定位。
本发明采用的技术方案是：一种基于EKF的多传感器融合温室巡检机器人跟踪方法。其特征包括如下步骤：
步骤1：基于UWB测距模块的基站到目标距离信息获取：基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息；
步骤2：目标机器人加速度，航向角信息获取：利用MPU6050传感器，经四元数计算获取机器人的加速度和航向角信息；
步骤3：初始观测坐标获取：使用三边定位模型，基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值；
步骤4：坐标观测值修正：利用泰勒展开在观测点进行迭代修正，当修正值小于设定阈值或达到迭代次数时结束迭代；
步骤5：融合定位：将修正后的坐标观测值，实时测量的机器人加速度和航向角同时传入CTRV运动模型，利用扩展的卡尔曼滤波对传感器数据进行融合处理，得到目标机器人的最优估计坐标。
进一步，步骤3具体过程为：
已知固定的三个基站坐标分别为(x1,y1),(x2,y2),(x3,y3),设初始观测目标位置为(x0,y0),由步骤1中UWB测距模块测得三个基站到目标(x0,y0)的距离分别为d1,d2,d3,则由三边定位模型可知：
Figure BDA0002444397930000021
Figure BDA0002444397930000022
Figure BDA0002444397930000023
上述三式两两相减得到线性方程，写成矩阵形式为：
Figure BDA0002444397930000031
记作：
X·θ＝Y (45)
由最小二乘法解得初始观测坐标：
Figure BDA0002444397930000032
其中XT为X的转置矩阵，(XTX)-1意为对X′和X矩阵相乘的结果进行求逆矩阵运算。
进一步，步骤4具体过程为：
设(x,y)为修正后的观测坐标，则(x,y)到3个基站的距离分别记作：
Figure BDA0002444397930000033
Figure BDA0002444397930000034
Figure BDA0002444397930000035
且
f1(x0,y0)＝d1 (50)
f2(x0,y0)＝d2 (51)
f3(x0,y0)＝d3 (52)
将fn(x,y)在(x0,y0)处进行泰勒展开，将第一次使用Taylor算法优化得到的观测值令P1＝[x1 y1]T,将由步骤3得到的初始观测坐标表示为P0＝[x0 y0]T，设优化观测值与初始观测值之差为ΔP1-0＝P1-P0＝[Δx1-0 Δy1-0]T,用矩阵形式表达为：
Figure BDA0002444397930000041
记式(53)为：
F0·ΔP1-0＝η1-0 (54)
则由最小二乘法解得修正值ΔP1-0为：
Figure BDA0002444397930000042
则这一轮泰勒展开计算后得到的新的观测坐标值为P1＝P0+ΔP1-0，重复上述泰勒展开计算，迭代步骤可以表示为：
将上一次经Taylor算法优化后得到的值设为Pk-1＝[xk-1 yk-1]T，作为下一次迭代的初始观测值，将下一次迭代后得到的优化观测值设为Pk＝[xk yk]T，设优化观测值与初始观测值之差为ΔPk-(k-1)＝Pk-Pk-1＝[Δxk-(k-1) Δyk-(k-1)]T，将fn(x,y)在Pk-1处进行泰勒展开，用矩阵形式表达为：
Figure BDA0002444397930000043
将式(56)记作：
Fk-1·ΔPk-(k-1)＝ηk-(k-1) (57)
则由最小二乘法解得修正值ΔPk-(k-1)为：
Figure BDA0002444397930000051
重复上述迭代步骤，直到ΔPk-(k-1)≤ε，其中ε为提前设定的经验阈值，满足此条件时认为算法收敛，得到优化后的观测坐标值Pk：
Pk＝Pk-1+ΔPk-(k-1) (59)
进一步，步骤5具体过程为：
将步骤4得到的修正后的坐标观测值(x,y)和MPU6050传感器测得的航向角θ传入恒定转率和速度模型(CTRV运动模型)，可以将目标机器人的状态表示为：
X＝[x y v θ ω]T (60)
其中，x代表目标机器人横坐标，y代表纵坐标，v代表运动速度，θ代表航向角，ω代表角速度，在CTRV运动模型中，目标的运动速度v和角速度ω被假设为恒定值，而模型的噪声主要来自于直线加速度a和角加速度
Figure BDA0002444397930000052
由于扩展的卡尔曼滤波的工作是由第k次的结果预测第k+1次的结果，结合第k+1次的测量数据对预测结果进行修正，得到第k+1次的最终结果。CTRV运动模型的状态转移函数为：
Figure BDA0002444397930000053
上式给出了第k次的最终结果Xk＝[xk yk vk θk ωk]T与根据第k次预测得到的第k+1次的预测值
Figure BDA0002444397930000054
将
Figure BDA0002444397930000055
看作是自变量为Xk的函数f(Xk)。
当ω≠0时可以写成：
Figure BDA0002444397930000061
当ω＝0时可以写成：
Figure BDA0002444397930000062
利用一阶泰勒展开对模型在Xk进行线性化：
f(X)＝f(Xk)+(X-Xk)JA(Xk) (64)
式(64)中的JA(Xk)代表由f(Xk)的一阶偏导数组成的雅可比矩阵。
当ω≠0时，计算得到雅可比矩阵JA(Xk)为：
Figure BDA0002444397930000063
当ω≠0时，计算得到雅可比矩阵JA(Xk)为：
Figure BDA0002444397930000064
预测误差协方差表示为：
Figure BDA0002444397930000071
其中
Figure BDA0002444397930000072
是由上一个状态的误差协方差矩阵Pk得到的表示预测误差的协方差矩阵，Q代表噪声的协方差矩阵，而噪声主要来自于直线加速度a和角加速度
Figure BDA0002444397930000073
假定a和
Figure BDA0002444397930000074
均满足0均值的高斯分布，方差分别为
Figure BDA0002444397930000075
和
Figure BDA0002444397930000076
a和
Figure BDA0002444397930000077
对状态影响表达为W：
Figure BDA0002444397930000078
将式(68)表示为：
W＝G·λ (69)
则Q为处理噪声的协方差矩阵：
Q＝E[W·WT]＝G·E[λ·λT]·GT (70)
其中，E[W·WT]代表对W·WT求数学期望，E[λ·λT]则是对λ·λT求数学期望。
由于a和
Figure BDA0002444397930000079
均满足0均值的高斯分布，所以E[λ·λT]可以表示为：
Figure BDA00024443979300000710
此处假设
Figure BDA00024443979300000711
为定值0.3而
Figure BDA00024443979300000712
通过MPU6050传回的加速度值得到，处理噪声的协方差矩阵Q的计算公式为：
Figure BDA0002444397930000081
由步骤2和步骤4获取的线性测量模型为：
Z＝[x y θ]T (73)
将预测状态映射到测量模型空间的测量矩阵为：
Figure BDA0002444397930000082
测量噪声的协方差矩阵R为:
Figure BDA0002444397930000083
式(75)中的
Figure BDA0002444397930000084
分别表示测量模型Z＝[x y θ]T中x,y,θ测量值的方差，表示测量噪声，此值根据经验设置为定值。
整个步骤5可以叙述为：
根据上一次的状态Xk-1计算这一次的状态预测值
Figure BDA0002444397930000085
Figure BDA0002444397930000086
根据上一次的误差协方差矩阵Pk-1计算这一次的预测值与真实值之间的误差协方差矩阵
Figure BDA0002444397930000087
将式(65)中的雅可比矩阵简写为JA，其转置矩阵为JA T，Q为式(70)中的处理噪声的协方差矩阵，则
Figure BDA0002444397930000088
可以表示为：
Figure BDA0002444397930000091
计算这一次的卡尔曼增益Kk：
Figure BDA0002444397930000092
其中H是式(74)中将预测状态映射到测量模型空间的测量矩阵，R是式(75)中的测量噪声的协方差矩阵。
根据这一次的测量模型Zk＝[xk yk θk]T(其中xk，yk为步骤4得到的这一次的坐标观测值，θk为步骤2得到的这一次的航向角)更新状态，得到当前状态Xk：
Figure BDA0002444397930000093
更新误差协方差矩阵：
Figure BDA0002444397930000094
其中Pk为这一次的更新后的误差协方差矩阵。
本发明提出一种基于EKF的多传感器融合温室巡检机器人跟踪方法，通过采用上述技术方案后，其具有以下有益效果：
(1)本发明相对于传统的静态传感器网络来说，在温室精准农业中能以更少的传感器节点和更低的布置成本实现对温室植株的监测和作业。
(2)本发明相对于单一的UWB室内定位技术，本专利提出的一种基于EKF的多传感器融合温室巡检机器人跟踪方法，使用UWB测距技术和惯导结合的方案，使用扩展得卡尔曼滤波对数据进行融合，能够有效减少单一传感器偏差、非视距传播等带来的影响，提高了定位精度，实现了精确的目标跟踪。
(3)本发明与传统定位算法相比，使用了Taylor展开迭代优化的方法对观测值进行了优化预处理，再此优化观测值基础上利用扩展的卡尔曼滤波进行数据融合，得到最终的最优估计结果，提高了定位的精度。
附图说明
附图1为本发明UWB定位基站标签实物图
附图2为本发明算法流程图
附图3为本发明运动轨迹跟踪运行结果
附图4为本发明机器人温室巡检机器人定位系统上位机运行界面与定位结果
具体实施方式
本发明首先通过UWB测距模块基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息，同时通过机器人搭载的MPU6050实时获取机器人的加速度和航向角。基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值，利用Taylor算法在观测点进行迭代修正，将修正后的坐标观测值，实时测量的机器人加速度和航向角同时传入CTRV运动模型(恒定转率和速度模型)，利用扩展的卡尔曼滤波(EKF)对传感器数据进行融合处理，将扩展的卡尔曼滤波结果作为机器人当前位置输出，得到一个多传感器融合的温室巡检机器人跟踪方法。本发明能够实现对温室巡检机器人在室内环境下的精确定位，对机器人的运动轨迹进行跟踪，实时获取机器人坐标，与多光谱相机，湿度计等环境感知传感器结合，对整个温室植物生长状态和环境变化进行监控，减少温室管理的成本。
一种基于EKF的多传感器融合温室巡检机器人跟踪方法，包括如下步骤：
步骤1：基于UWB测距模块的基站到目标距离信息获取：基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息；
步骤2：目标机器人加速度，航向角信息获取：利用MPU6050传感器，经四元数计算获取机器人的加速度和航向角信息；
步骤3：初始观测坐标获取：使用三边定位模型，基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值；
步骤4：坐标观测值修正：利用泰勒展开在观测点进行迭代修正，当修正值小于设定阈值或达到迭代次数时结束迭代；
步骤5：融合定位：将修正后的坐标观测值，实时测量的机器人加速度和航向角同时传入CTRV运动模型，利用扩展的卡尔曼滤波对传感器数据进行融合处理，得到目标机器人的最优估计坐标。
上述步骤3具体过程为：
已知固定的三个基站坐标分别为(x1,y1),(x2,y2),(x3,y3),设初始观测目标位置为(x0,y0),由步骤1中UWB测距模块测得三个基站到目标(x0,y0)的距离分别为d1,d2,d3,则由三边定位模型可知：
Figure BDA0002444397930000111
Figure BDA0002444397930000112
Figure BDA0002444397930000113
上述三式两两相减得到线性方程，写成矩阵形式为：
Figure BDA0002444397930000114
记作：
X·θ＝Y (85)
由最小二乘法解得初始观测坐标：
Figure BDA0002444397930000115
其中XT为X的转置矩阵，(XTX)-1意为对X′和X矩阵相乘的结果进行求逆矩阵运算。
上述步骤4具体过程为：
设(x,y)为修正后的观测坐标，则(x,y)到3个基站的距离分别记作：
Figure BDA0002444397930000116
Figure BDA0002444397930000117
Figure BDA0002444397930000118
且
f1(x0,y0)＝d1 (90)
f2(x0,y0)＝d2 (91)
f3(x0,y0)＝d3 (92)
将fn(x,y)在(x0,y0)处进行泰勒展开，将第一次使用Taylor算法优化得到的观测值令P1＝[x1 y1]T,将由步骤3得到的初始观测坐标表示为P0＝[x0 y0]T，设优化观测值与初始观测值之差为ΔP1-0＝P1-P0＝[Δx1-0 Δy1-0]T,用矩阵形式表达为：
Figure BDA0002444397930000121
记式(13)为：
F0·ΔP1-0＝η1-0 (94)
则由最小二乘法解得修正值ΔP1-0为：
Figure BDA0002444397930000122
则这一轮泰勒展开计算后得到的新的观测坐标值为P1＝P0+ΔP1-0，重复上述泰勒展开计算，迭代步骤可以表示为：
将上一次经Taylor算法优化后得到的值设为Pk-1＝[xk-1 yk-1]T，作为下一次迭代的初始观测值，将下一次迭代后得到的优化观测值设为Pk＝[xk yk]T，设优化观测值与初始观测值之差为ΔPk-(k-1)＝Pk-Pk-1＝[Δxk-(k-1) Δyk-(k-1)]T，将fn(x,y)在Pk-1处进行泰勒展开，用矩阵形式表达为：
Figure BDA0002444397930000131
将式(16)记作：
Fk-1·ΔPk-(k-1)＝ηk-(k-1) (97)
则由最小二乘法解得修正值ΔPk-(k-1)为：
Figure BDA0002444397930000132
重复上述迭代步骤，直到ΔPk-(k-1)≤ε，其中ε为提前设定的经验阈值，满足此条件时认为算法收敛，得到优化后的观测坐标值Pk：
Pk＝Pk-1+ΔPk-(k-1) (99)
上述步骤5具体过程为：
将步骤4得到的修正后的坐标观测值(x,y)和MPU6050传感器测得的航向角θ传入恒定转率和速度模型(CTRV运动模型)，可以将目标机器人的状态表示为：
X＝[x y v θ ω]T (100)
其中，x代表目标机器人横坐标，y代表纵坐标，v代表运动速度，θ代表航向角，ω代表角速度，在CTRV运动模型中，目标的运动速度v和角速度ω被假设为恒定值，而模型的噪声主要来自于直线加速度a和角加速度
Figure BDA0002444397930000133
由于扩展的卡尔曼滤波的工作是由第k次的结果预测第k+1次的结果，结合第k+1次的测量数据对预测结果进行修正，得到第k+1次的最终结果，CTRV运动模型的状态转移函数为：
Figure BDA0002444397930000141
上式给出了第k次的最终结果Xk＝[xk yk vk θk ωk]T与根据第k次预测得到的第k+1次的预测值
Figure BDA0002444397930000142
将
Figure BDA0002444397930000143
看作是自变量为Xk的函数f(Xk)；
当ω≠0时可以写成：
Figure BDA0002444397930000144
当ω＝0时可以写成：
Figure BDA0002444397930000145
利用一阶泰勒展开对模型在Xk进行线性化：
f(X)＝f(Xk)+(X-Xk)JA(Xk) (104)
式(24)中的JA(Xk)代表由f(Xk)的一阶偏导数组成的雅可比矩阵；
当ω≠0时，计算得到雅可比矩阵JA(Xk)为：
Figure BDA0002444397930000151
当ω≠0时，计算得到雅可比矩阵JA(Xk)为：
Figure BDA0002444397930000152
预测误差协方差表示为：
Figure BDA0002444397930000153
其中
Figure BDA0002444397930000154
是由上一个状态的误差协方差矩阵Pk得到的表示预测误差的协方差矩阵，Q代表噪声的协方差矩阵，而噪声主要来自于直线加速度a和角加速度
Figure BDA0002444397930000155
假定a和
Figure BDA0002444397930000156
均满足0均值的高斯分布，方差分别为
Figure BDA0002444397930000157
和
Figure BDA0002444397930000158
a和
Figure BDA0002444397930000159
对状态影响表达为W：
Figure BDA00024443979300001510
将式(28)表示为：
W＝G·λ (109)
则Q为处理噪声的协方差矩阵：
Q＝E[W·WT]＝G·E[λ·λT]·GT (110)
其中，E[W·WT]代表对W·WT求数学期望，E[λ·λT]则是对λ·λT求数学期望；
由于a和
Figure BDA0002444397930000161
均满足0均值的高斯分布，所以E[λ·λT]可以表示为：
Figure BDA0002444397930000162
此处假设
Figure BDA0002444397930000163
为定值0.3而
Figure BDA0002444397930000164
通过MPU6050传回的加速度值得到，处理噪声的协方差矩阵Q的计算公式为：
Figure BDA0002444397930000165
由步骤2和步骤4获取的线性测量模型为：
Z＝[x y θ]T (113)
将预测状态映射到测量模型空间的测量矩阵为：
Figure BDA0002444397930000166
测量噪声的协方差矩阵R为:
Figure BDA0002444397930000167
式(35)中的
Figure BDA0002444397930000168
分别表示测量模型Z＝[x y θ]T中x,y,θ测量值的方差，表示测量噪声，此值根据经验设置为定值；
整个步骤5可以叙述为：
根据上一次的状态Xk-1计算这一次的状态预测值
Figure BDA0002444397930000171
Figure BDA0002444397930000172
根据上一次的误差协方差矩阵Pk-1计算这一次的预测值与真实值之间的误差协方差矩阵
Figure BDA0002444397930000173
将式(25)中的雅可比矩阵简写为JA，其转置矩阵为JA T，Q为式(30)中的处理噪声的协方差矩阵，则
Figure BDA0002444397930000174
可以表示为：
Figure BDA0002444397930000175
计算这一次的卡尔曼增益Kk：
Figure BDA0002444397930000176
其中H是式(34)中将预测状态映射到测量模型空间的测量矩阵，R是式(35)中的测量噪声的协方差矩阵；
根据这一次的测量模型Zk＝[xk yk θk]T更新状态，其中xk，yk为步骤4得到的这一次的坐标观测值，θk为步骤2得到的这一次的航向角，得到当前状态Xk：
Figure BDA0002444397930000177
更新误差协方差矩阵：
Figure BDA0002444397930000178
其中Pk为这一次的更新后的误差协方差矩阵。
下面结合附图具体说明本发明提出的一种基于EKF的多传感器融合温室巡检机器人跟踪方法在温室巡检机器人精确定位，轨迹跟踪中的应用，其具体实施方式如下：
附图1所示为UWB定位基站标签实物图。其中3个基站被放置于固定位置，温室巡检机器人搭载了标签，通过UWB测距模块利用DS-TWR测距技术可以获取各基站到目标的距离。其中基站和标签均使用DWM1000作为通信模块完成信息交互，通过SPI协议与STM32F103RBT6主控单片机通信，在3个基站依次完成与标签之间的测距之后，基站2和基站3将距离信息通过UWB通信传送至基站1，基站1主控板集成了wifi模块，在完成一次3个基站与标签测距信息汇总后，将测距值打包发送至上位机。
温室巡检机器人采用STM32F407VIT6作为主控芯片，使用IIC协议与MPU6050进行通信，通过四元数结算获取温室巡检机器人当前的加速度和航向角，通过wifi发送至上位机。
如附图2所示，本发明提出的基于EKF的多传感器融合温室巡检机器人跟踪方法算法流程图。在上位机通过wifi接受到一次测距、加速度、航向角信息后，首先利用三边定位算法，经过线性化处理后加以最小二乘法解算，得到最初的坐标观测值，记为θ＝[x0 y0]T，利用Taylor展开在θ处进行展开，使用最小二乘法进行解算，经过实际验证，在经过10次迭代后，Pk＝Pk-1+ΔP式中的ΔP已经变的极小，故在实际使用时固定迭代十次，将最后一次修正后的结果P10作为优化后的坐标观测值输出。在经过Taylor迭代算法优化后，再将坐标观测值、加速度和航向角一并作为CTRV运动模型的输入，因为CTRV并非是线性模型，故使用扩展的卡尔曼滤波，利用泰勒展开对CTRV模型进行线性化，经过预测-更新迭代获得最终的目标最优估计值。
附图3为机器人运动轨迹跟踪运行结果。在附图3进行的测试中，上位机中显示出机器人行走轨迹，在经过本发明提出的基于EKF的多传感器融合温室巡检机器人跟踪方法优化后，跟踪效果良好，经测试定位误差在5cm到10cm。
附图4为温室巡检机器人定位系统上位机运行界面与定位结果。在模拟的温室环境中，巡检车的实时位置经过本发明提出的基于EKF的多传感器融合温室巡检机器人跟踪方法优化后显示在了上位机界面上(图中的黑点表示的即为机器人位置)，在机器人在模拟温室中进行巡检时，会同时进行植株病害检测(图中模拟温室中黄色泡沫代表病害植株)，当检测到病害时，会将病害信息传至上位机，并将病害位置和病害类型在上位机中显示出来。
综上所述，本发明公开了一种基于EKF的多传感器融合温室巡检机器人跟踪方法，属于计算机控制工程领域。本发明首先使用UWB基站和标签获取固定基站到温室巡检机器人的距离，使用MPU6050获取温室巡检机器人的加速度和航向角，实时地通过wifi发送至上位机，上位机使用本发明提出的一种基于EKF的多传感器融合温室巡检机器人跟踪方法对传感器信息进行处理。在经过最小二乘法和Taylor迭代算法预处理后，观测值坐标精度被有效提高，最后使用扩展的卡尔曼滤波算法对优化后的观测值进行迭代输出，温室巡检机器人运动轨迹变得平滑，更加接近真实运动轨迹，有效提高了室内定位和目标跟踪精度，温室巡检机器人搭载各种传感器成为动态的传感器节点，相比传统的静态传感器网络，大大减少了成本，简化了布置传感器网络的工作。
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (2)

1.一种基于EKF的多传感器融合温室巡检机器人跟踪方法，其特征包括如下步骤：
步骤1：基于UWB测距模块的基站到目标距离信息获取：基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息；
步骤2：目标机器人加速度，航向角信息获取：利用MPU6050传感器，经计算获取机器人的加速度和航向角信息；
步骤3：初始观测坐标获取：使用三边定位模型，基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值；
步骤4：坐标观测值修正：利用泰勒展开在观测点进行迭代修正，当修正值小于设定阈值或达到迭代次数时结束迭代；
步骤5：融合定位：将修正后的坐标观测值，实时测量的机器人加速度和航向角同时传入CTRV运动模型，利用扩展的卡尔曼滤波对传感器数据进行融合处理，得到目标机器人的最优估计坐标；
步骤4具体过程为：
设(x,y)为修正后的观测坐标，则(x,y)到3个基站的距离分别记作：
Figure FDA0003818099830000011
Figure FDA0003818099830000012
Figure FDA0003818099830000013
且
f1(x0,y0)＝d1 (10)
f2(x0,y0)＝d2 (11)
f3(x0,y0)＝d3 (12)
将fn(x,y)在(x0,y0)处进行泰勒展开，将第一次使用Taylor算法优化得到的观测值令P1＝[x1 y1]T,将由步骤3得到的初始观测坐标表示为P0＝[x0 y0]T，设优化观测值与初始观测值之差为ΔP1-0＝P1-P0＝[Δx1-0 Δy1-0]T,用矩阵形式表达为：
Figure FDA0003818099830000021
记式(13)为：
F0·ΔP1-0＝η1-0 (14)
则由最小二乘法解得修正值ΔP1-0为：
Figure FDA0003818099830000022
则这一轮泰勒展开计算后得到的新的观测坐标值为P1＝P0+ΔP1-0，重复上述泰勒展开计算，迭代步骤可以表示为：
将上一次经Taylor算法优化后得到的值设为Pk-1＝[xk-1 yk-1]T，作为下一次迭代的初始观测值，将下一次迭代后得到的优化观测值设为Pk＝[xk yk]T，设优化观测值与初始观测值之差为ΔPk-(k-1)＝Pk-Pk-1＝[Δxk-(k-1) Δyk-(k-1)]T，将fn(x,y)在Pk-1处进行泰勒展开，用矩阵形式表达为：
Figure FDA0003818099830000023
将式(16)记作：
Fk-1·ΔPk-(k-1)＝ηk-(k-1) (17)
则由最小二乘法解得修正值ΔPk-(k-1)为：
Figure FDA0003818099830000031
重复上述迭代步骤，直到ΔPk-(k-1)≤ε，其中ε为提前设定的经验阈值，满足此条件时认为算法收敛，得到优化后的观测坐标值Pk：
Pk＝Pk-1+ΔPk-(k-1) (19)；
步骤5具体过程为：
将步骤4得到的修正后的坐标观测值(x,y)和MPU6050传感器测得的航向角θ传入恒定转率和速度模型(CTRV运动模型)，可以将目标机器人的状态表示为：
X＝[x y v θ ω]T (20)
其中，x代表目标机器人横坐标，y代表纵坐标，v代表运动速度，θ代表航向角，ω代表角速度，在CTRV运动模型中，目标的运动速度v和角速度ω被假设为恒定值，而模型的噪声主要来自于直线加速度a和角加速度
Figure FDA0003818099830000032
由于扩展的卡尔曼滤波的工作是由第k次的结果预测第k+1次的结果，结合第k+1次的测量数据对预测结果进行修正，得到第k+1次的最终结果，CTRV运动模型的状态转移函数为：
Figure FDA0003818099830000033
上式给出了第k次的最终结果Xk＝[xk yk vk θk ωk]T与根据第k次预测得到的第k+1次的预测值
Figure FDA0003818099830000034
将
Figure FDA0003818099830000035
看作是自变量为Xk的函数f(Xk)；
当ω≠0时可以写成：
Figure FDA0003818099830000041
当ω＝0时可以写成：
Figure FDA0003818099830000042
利用一阶泰勒展开对模型在Xk进行线性化：
f(X)＝f(Xk)+(X-Xk)JA(Xk) (24)
式(24)中的JA(Xk)代表由f(Xk)的一阶偏导数组成的雅可比矩阵；
当ω≠0时，计算得到雅可比矩阵JA(Xk)为：
Figure FDA0003818099830000043
当ω≠0时，计算得到雅可比矩阵JA(Xk)为：
Figure FDA0003818099830000051
预测误差协方差表示为：
Figure FDA0003818099830000052
其中
Figure FDA0003818099830000053
是由上一个状态的误差协方差矩阵Pk得到的表示预测误差的协方差矩阵，Q代表噪声的协方差矩阵，而噪声主要来自于直线加速度a和角加速度
Figure FDA0003818099830000054
假定a和
Figure FDA0003818099830000055
均满足0均值的高斯分布，方差分别为
Figure FDA0003818099830000056
和
Figure FDA0003818099830000057
a和
Figure FDA0003818099830000058
对状态影响表达为W：
Figure FDA0003818099830000059
将式(28)表示为：
W＝G·λ (29)
则Q为处理噪声的协方差矩阵：
Q＝E[W·WT]＝G·E[λ·λT]·GT (30)
其中，E[W·WT]代表对W·WT求数学期望，E[λ·λT]则是对λ·λT求数学期望；
由于a和
Figure FDA00038180998300000510
均满足0均值的高斯分布，所以E[λ·λT]可以表示为：
Figure FDA00038180998300000511
此处假设
Figure FDA00038180998300000512
为定值0.3而
Figure FDA00038180998300000513
通过MPU6050传回的加速度值得到，处理噪声的协方差矩阵Q的计算公式为：
Figure FDA0003818099830000061
由步骤2和步骤4获取的线性测量模型为：
Z＝[x y θ]T (33)
将预测状态映射到测量模型空间的测量矩阵为：
Figure FDA0003818099830000062
测量噪声的协方差矩阵R为:
Figure FDA0003818099830000063
式(35)中的
Figure FDA0003818099830000064
分别表示测量模型Z＝[x y θ]T中x,y,θ测量值的方差，表示测量噪声，此值根据经验设置为定值；
整个步骤5可以叙述为：
根据上一次的状态Xk-1计算这一次的状态预测值
Figure FDA0003818099830000065
Figure FDA0003818099830000066
根据上一次的误差协方差矩阵Pk-1计算这一次的预测值与真实值之间的误差协方差矩阵
Figure FDA0003818099830000067
将式(25)中的雅可比矩阵简写为JA，其转置矩阵为JA T，Q为式(30)中的处理噪声的协方差矩阵，则
Figure FDA0003818099830000068
可以表示为：
Figure FDA0003818099830000071
计算这一次的卡尔曼增益Kk：
Figure FDA0003818099830000072
其中H是式(34)中将预测状态映射到测量模型空间的测量矩阵，R是式(35)中的测量噪声的协方差矩阵；
根据这一次的测量模型Zk＝[xk yk θk]T更新状态，其中xk，yk为步骤4得到的这一次的坐标观测值，θk为步骤2得到的这一次的航向角，得到当前状态Xk：
Figure FDA0003818099830000073
更新误差协方差矩阵：
Figure FDA0003818099830000074
其中Pk为这一次的更新后的误差协方差矩阵。
2.根据权利要求1所述的一种基于EKF的多传感器融合温室巡检机器人跟踪方法，其特征在于，步骤3具体过程为：
已知固定的三个基站坐标分别为(x1,y1),(x2,y2),(x3,y3),设初始观测目标位置为(x0,y0),由步骤1中UWB测距模块测得三个基站到目标(x0,y0)的距离分别为d1,d2,d3,则由三边定位模型可知：
Figure FDA0003818099830000075
Figure FDA0003818099830000076
Figure FDA0003818099830000077
上述三式两两相减得到线性方程，写成矩阵形式为：
Figure FDA0003818099830000078
记作：
X·θ＝Y (5)
由最小二乘法解得初始观测坐标：
Figure FDA0003818099830000081
其中XT为X的转置矩阵，(XTX)-1意为对X′和X矩阵相乘的结果进行求逆矩阵运算。
CN202010274834.7A 2020-04-09 2020-04-09 一种基于ekf的多传感器融合温室巡检机器人跟踪方法 Active CN111536967B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010274834.7A CN111536967B (zh) 2020-04-09 2020-04-09 一种基于ekf的多传感器融合温室巡检机器人跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010274834.7A CN111536967B (zh) 2020-04-09 2020-04-09 一种基于ekf的多传感器融合温室巡检机器人跟踪方法

Publications (2)

Publication Number Publication Date
CN111536967A CN111536967A (zh) 2020-08-14
CN111536967B true CN111536967B (zh) 2022-12-16

Family

ID=71977178

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010274834.7A Active CN111536967B (zh) 2020-04-09 2020-04-09 一种基于ekf的多传感器融合温室巡检机器人跟踪方法

Country Status (1)

Country Link
CN (1) CN111536967B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111967094B (zh) * 2020-09-01 2022-08-16 吉林大学 一种基于Mobileye提供车道线方程的后向车道线推算方法
CN112033412A (zh) * 2020-09-07 2020-12-04 中国南方电网有限责任公司超高压输电公司天生桥局 一种提高巡检机器人定位精度的方法及装置
CN112472432B (zh) * 2020-11-25 2023-06-30 武汉理工大学 一种手杖-轮椅自动跟随系统及方法
CN112862161A (zh) * 2021-01-18 2021-05-28 上海燕汐软件信息科技有限公司 货物分拣管理方法、装置、电子设备和存储介质
CN113361562B (zh) * 2021-04-20 2024-03-15 武汉理工大学 一种用于动力电池反应控制模块的多传感器融合方法及装置
CN113610263B (zh) * 2021-06-18 2023-06-09 广东能源集团科学技术研究院有限公司 一种电厂作业车辆轨迹估计方法及系统
CN113352300B (zh) * 2021-06-28 2024-06-18 广东工业大学 一种喷涂机器人示教器及方法
CN113627473B (zh) * 2021-07-06 2023-09-29 哈尔滨工程大学 基于多模态传感器的水面无人艇环境信息融合感知方法
CN113766103B (zh) * 2021-07-22 2023-08-08 宁波思高信通科技有限公司 一种巡检机器人的室内定位方法
CN115267868B (zh) * 2022-09-27 2023-09-19 腾讯科技（深圳）有限公司 一种定位点处理方法、装置及计算机可读存储介质

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108120438B (zh) * 2017-12-15 2020-05-05 北京工商大学 一种基于imu和rfid信息融合的室内目标快速跟踪方法
CN108711163A (zh) * 2018-02-24 2018-10-26 中国人民解放军火箭军工程大学 一种基于多传感器的机器人未知环境目标跟踪方法
CN108733055A (zh) * 2018-05-18 2018-11-02 郑州万达科技发展有限公司 一种多传感器数据融合的方法及agv导航定位装置
CN108955679B (zh) * 2018-08-16 2022-03-15 电子科技大学 一种变电站智能巡检机器人高精度定位方法
CN110602647B (zh) * 2019-09-11 2020-11-24 江南大学 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法

Also Published As

Publication number Publication date
CN111536967A (zh) 2020-08-14

Similar Documents

Publication Publication Date Title
CN111536967B (zh) 一种基于ekf的多传感器融合温室巡检机器人跟踪方法
Li et al. UWB-based localization system aided with inertial sensor for underground coal mine applications
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN103471595B (zh) 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN107014375B (zh) 超低部署的室内定位系统及方法
CN102322858B (zh) 用于地磁/捷联惯导组合导航系统的地磁匹配导航方法
CN106030430A (zh) 用于使用旋翼微型航空载具（mav）在室内和室外环境中的稳健的自主飞行的多传感器融合
Zhou et al. A graph optimization-based indoor map construction method via crowdsourcing
CN106772524A (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN108362288A (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN108387236A (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN114485643B (zh) 一种煤矿井下移动机器人环境感知与高精度定位方法
CN110275193A (zh) 一种基于因子图的集群卫星协同导航方法
CN113639722B (zh) 连续激光扫描配准辅助惯性定位定姿方法
Zhou et al. A fusion method of pedestrian dead reckoning and pseudo indoor plan based on conditional random field
Xu et al. $ D^ 2$ SLAM: Decentralized and Distributed Collaborative Visual-inertial SLAM System for Aerial Swarm
Lin et al. Drift-free visual slam for mobile robot localization by integrating uwb technology
Jiang et al. 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments
CN111121818B (zh) 一种无人车中相机与二维码的标定方法
CN115950414A (zh) 一种不同传感器数据的自适应多重融合slam方法
CN113503891B (zh) 一种sinsdvl对准校正方法、系统、介质及设备
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
Ge et al. Integrating WSN and Laser SLAM for Mobile Robot Indoor Localization.
CN109269499B (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