一种基于EKF的多传感器融合温室巡检机器人跟踪方法技术

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

【技术实现步骤摘要】
一种基于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,则由三边定位模型可知：上述三式两两相减得到线性方程，写成矩阵形式为：记作：X·θ＝Y(45)由最小二乘法解得初始观测坐标：其中XT为X的转置矩阵，(XTX)-1意为对X′和X矩阵相乘的结果进行求逆矩阵运算。进一步，步骤4具体过程为：设(x,y)为修正后的观测坐标，则(x,y)到3个基站的距离分别记作：且f1(x0,y0)＝d1(50)f2(x0,y0)＝d2(51)f3(x0,y0)＝d3(52)将fn(x,y)在(x0,y0)处进行泰勒展开，将第一次使用Taylor算法优化得到的观测值令P1＝[x1y1]T,将由步骤3得到的初始观测坐标表示为P0＝[x0y0]T，设优化观测值与初始观测值之差为ΔP1-0＝P1-P0＝[Δx1-0Δy1-0]T,用矩阵形式表达为：记式(53)为：F0·ΔP1-0＝η1-0(54)则由最小二乘法解得修正值ΔP1-0为：则这一轮泰勒展开计算后得到的新的观测坐标值为P1＝P0+ΔP1-0，重复上述泰勒展开计算，迭代步骤可以表示为：将上一次经Taylor算法优化后得到的值设为Pk-1＝[xk-1yk-1]T，作为下一次迭代的初始观测值，将下一次迭代后得到的优化观测值设为Pk＝[xkyk]T，设优化观测值与初始观测值之差为ΔPk-(k-1)＝Pk-Pk-1＝[Δxk-(k-1)Δyk-(k-1)]T，将fn(x,y)在Pk-1处进行泰勒展开，用矩阵形式表达为：将式(56)记作：Fk-1·ΔPk-(k-1)＝ηk-(k-1)(57)则由最小二乘法解得修正值ΔPk-(k-1)为：重复上述迭代步骤，直到ΔPk-(k-1)≤ε，其中ε为提前设定的经验阈值，满足此条件时认为算法收敛，得到优化后的观测坐标值Pk：Pk＝Pk-1+ΔPk-(k-1)(59)进一步，步骤5具体过程为：将步骤4得到的修正后的坐标观测值(x,y)和MPU6050传感器测得的航向角θ传入恒定转率和速度模型(CTRV运动模型)，可以将目标机器人的状态表示为：X＝[xyvθω]T(60)其中，x代表目标机器人横坐标，y代表纵坐标，v代表运动速度，θ代表航向角，ω代表角速度，在CTRV运动模型中，目标的运动速度v和角速度ω被假设为恒定值，而模型的噪声主要来自于直线加速度a和角加速度由于扩展的卡尔曼滤波的工作是由第k次的结果预测第k+1次的结果，结合第k+1次的测量数据对预测结果进行修正，得到第k+1次的最终结果。CTRV运动模型的状态转移函数为：上式给出了第k次的最终结果Xk＝[xkykvkθkωk]T与根据第k次预测得到的第k+1次的预测值将看作是自变量为Xk的函数f(Xk)。当ω≠0时可以写成：当ω＝0时可以写成：利用一阶泰勒展开对模型在Xk进行线性化：f(X)＝f(Xk)+(X-Xk)JA(Xk)(64)式(64)中本文档来自技高网...

【技术保护点】
1.一种基于EKF的多传感器融合温室巡检机器人跟踪方法。其特征包括如下步骤：/n步骤1：基于UWB测距模块的基站到目标距离信息获取：基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息；/n步骤2：目标机器人加速度，航向角信息获取：利用MPU6050传感器，经计算获取机器人的加速度和航向角信息；/n步骤3：初始观测坐标获取：使用三边定位模型，基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值；/n步骤4：坐标观测值修正：利用泰勒展开在观测点进行迭代修正，当修正值小于设定阈值或达到迭代次数时结束迭代；/n步骤5：融合定位：将修正后的坐标观测值，实时测量的机器人加速度和航向角同时传入CTRV运动模型，利用扩展的卡尔曼滤波对传感器数据进行融合处理，得到目标机器人的最优估计坐标。/n

【技术特征摘要】
1.一种基于EKF的多传感器融合温室巡检机器人跟踪方法。其特征包括如下步骤：
步骤1：基于UWB测距模块的基站到目标距离信息获取：基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息；
步骤2：目标机器人加速度，航向角信息获取：利用MPU6050传感器，经计算获取机器人的加速度和航向角信息；
步骤3：初始观测坐标获取：使用三边定位模型，基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值；
步骤4：坐标观测值修正：利用泰勒展开在观测点进行迭代修正，当修正值小于设定阈值或达到迭代次数时结束迭代；
步骤5：融合定位：将修正后的坐标观测值，实时测量的机器人加速度和航向角同时传入CTRV运动模型，利用扩展的卡尔曼滤波对传感器数据进行融合处理，得到目标机器人的最优估计坐标。


2.根据权利要求1所述的一种基于EKF的多传感器融合温室巡检机器人跟踪方法，其特征在于，步骤3具体过程为：
已知固定的三个基站坐标分别为(x1,y1),(x2,y2),(x3,y3),设初始观测目标位置为(x0,y0),由步骤1中UWB测距模块测得三个基站到目标(x0,y0)的距离分别为d1,d2,d3,则由三边定位模型可知：









上述三式两两相减得到线性方程，写成矩阵形式为：



记作：
X·θ＝Y(5)
由最小二乘法解得初始观测坐标：



其中XT为X的转置矩阵，(XTX)-1意为对X′和X矩阵相乘的结果进行求逆矩阵运算。


3.根据权利要求2所述的一种基于EKF的多传感器融合温室巡检机器人跟踪方法，其特征在于，步骤4具体过程为：
设(x,y)为修正后的观测坐标，则(x,y)到3个基站的距离分别记作：









且
f1(x0,y0)＝d1(10)
f2(x0,y0)＝d2(11)
f3(x0,y0)＝d3(12)
将fn(x,y)在(x0,y0)处进行泰勒展开，将第一次使用Taylor算法优化得到的观测值令P1＝[x1y1]T,将由步骤3得到的初始观测坐标表示为P0＝[x0y0]T，设优化观测值与初始观测值之差为ΔP1-0＝P1-P0＝[Δx1-0Δy1-0]T,用矩阵形式表达为：



记式(13)为：
F0·ΔP1-0＝η1-0(14)
则由最小二乘法解得修正值ΔP1-0为：



则这一轮泰勒展开计算后得到的新的观测坐标值为P1＝P0+ΔP1-0，重复上述泰勒展开计算，迭代步骤可以表示为：
将上一次经Taylor算法优化后得到的值设为Pk-1＝[xk-1yk-1]T，作为下一次迭代的初始观测值，将下一次迭代后得到的优化观测值设为Pk＝[xkyk]T，设优化观测值与初始观测值之差为ΔPk-(k-1)＝Pk-Pk-1＝[Δxk-(k-1)Δyk-(k-1)]T，将fn(x,y)在Pk-1处进行泰勒展开，用矩阵形式表达为：



将式(16)记作：
Fk-1·ΔPk-(k-1)＝ηk-(k-1)(17)
则由最小二乘法解得修正值ΔPk-(k-1)为：



重复上述迭代步骤，直到ΔPk-(k-1)≤ε，其中ε为提前设定的经验阈值，满足此条件时认为算法收敛，得到优化后的观测坐标值Pk：
Pk＝Pk-1+ΔPk-(k-1)(19)


4.根据权...

【专利技术属性】
技术研发人员：潘逸轩，姚瑶，王增辉，
申请(专利权)人：江苏大学，
类型：发明
国别省市：江苏;32