[发明专利]一种基于EKF的多传感器融合温室巡检机器人跟踪方法

说明书

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

技术领域

本发明涉及一种基于EKF(扩展的卡尔曼滤波)的多传感器融合温室巡检机器人跟踪方法，可应用在室内机器人精确定位、温室巡检机器人轨迹跟踪等场景。

背景技术

随着温室大棚的逐年增加，温室的无人化管理已经成为了温室智能化、机械化发展的必然趋势，传统的温室由人来管理，而人的进出使温室很难和外界环境隔开，人容易将病虫害带入温室，且温室的管理琐碎而繁杂，会耗费人大量的精力，所以无人化管理几乎是温室发展的必然方向。温室巡检机器人作为温室无人管理系统重要的组成成分，是未来实现无人化农业，精准农业关键。温室巡检机器人是集温室植株信息收集，环境感知，管理作业等功能于一体的综合系统，能够大大减少温室管理人工成本，提高温室管理智能化程度。由于温室巡检机器人需要在温室中对植株进行监测作业，如何准确到达指定植株，如何分辨温室机器人采集的传感器信息是属于哪一株植株的，想要解决上述问题，就必须要获取机器人精确的位置信息，因此，在室内情况下的目标精确定位成为温室巡检机器人自主控制领域乃至室内机器人控制领域的核心问题之一。

随着温室精细农业的迅猛发展，众多学者开始了温室内的定位研究，WIDODO等在温室中设计了基于声波的定位系统自校准方法；LUO等利用RSSI测距技术进行了温室内的定位研究以及王俊等利用BP算法进行温室内的定位研究。但上述方法为提高精度,就必须安装大量的辅助节点，但增大了系统建设的成本和能耗。在测量误差和能源消耗不可避免的情况下,减小测量误差和控制能耗显得尤为重要。本专利提出使用UWB室内定位技术和惯导融合定位的方法对温室巡检机器人进行室内精确定位。UWB在众多室内定位技术中脱颖而出，具有精度高、发射功率小、辐射小、成本低、网络节点布置方便的优点，不仅可以作为无线通信传感器网络使用，同时可以作为定位系统使用，适合在温室等室内环境中使用，但单一的UWB室内定位的精度会受传感器偏差、灵敏度误差、非视距传播影响，本专利提出一种基于EKF的多传感器融合温室巡检机器人跟踪方法，提高了定位精度，实现了精确的目标跟踪。

发明内容

发明目的：随着温室大棚的逐年增加，温室的无人化管理已经成为了温室智能化、机械化发展的必然趋势，而室内精准定位是实现对单一植株的精准监控和作业的基本需求，但是传统的无线传感器网络往往是固定的静态节点，需要布置大量传感器来对整个温室进行监控，而采用机器人搭载传感器的方案则需要面对更加严苛的动态定位精度要求。本发明提出一种基于EKF的多传感器融合温室巡检机器人跟踪方法，基于EKF利用UWB测距技术与惯导进行融合定位。

本发明采用的技术方案是：一种基于EKF的多传感器融合温室巡检机器人跟踪方法。其特征包括如下步骤：

步骤1：基于UWB测距模块的基站到目标距离信息获取：基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息；

步骤2：目标机器人加速度，航向角信息获取：利用MPU6050传感器，经四元数计算获取机器人的加速度和航向角信息；

步骤3：初始观测坐标获取：使用三边定位模型，基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值；

步骤4：坐标观测值修正：利用泰勒展开在观测点进行迭代修正，当修正值小于设定阈值或达到迭代次数时结束迭代；