一种基于自主导航的畜禽信息感知机器人与地图构建方法与流程

本发明涉及一种针对畜禽的智能农业装备和方法，具体涉及一种基于自主导航的畜禽信息感知机器人与地图构建方法。

背景技术：

随着社会的不断发展，集约化、规模化和智能化是养殖业发展的必然趋势。这种养殖结构一方面可以有效提高生产力，节约社会资源，降低成本，带来更高的社会经济效益；另一方面也符合动物福利的要求，是实施标准化生产、提高畜产品质量的必要基础。动物行为、环境因子等信息是规模养殖生产过程的重要指标，如何高效采集并管理这些信息为精准化、智能化养殖生产带来新的挑战。

目前我国畜禽业主要采用人工巡检、定点监测和移动式巡检的方式进行信息采集。人工巡检需要饲养员每天定时进行巡视和检查，其劳动强度高、监测效率低且具有一定危险性也不符合动物福利要求。定点监测是在固定位置安装检测设备，其成本高、监测参数少、灵活性差，有较大的局限性。移动式巡检主要将巡检设备放置在轨道上，通过遥控等方式控制其在轨道的运动。但轨道的铺设难度大、成本高、适用性低，不便在现有养殖环境进行二次铺轨。随着养殖行业逐渐向大型集约化、规模化的生产模式进行转型，相对结构化的畜禽舍构造在国内得到了迅速推广，这也使得机器人技术可以在畜禽养殖行业中可以得到广泛的应用。近年来也有相关学者不断尝试将机器人技术、人工智能技术、信息处理技术等应用到畜禽养殖行业，但仍需要饲养员进入禽舍近距离发出控制信号，智能程度较低，应用潜力不大。而畜禽舍内相对结构化的环境为机器人技术的应用提供便利条件，使得智能机器人自主导航技术能更好地完成畜禽舍中的任务。但畜禽舍的对称式结构，也导致了定位过程的对称模式，使得机器人无法确定当前的唯一位姿，无法准确匹配所监测的动物信息、环境信息与机器人在禽舍内的相对位置。

技术实现要素：

为了克服现存技术的弊端和解决背景技术中存在的问题，为有效解决畜禽舍环境对称结构下，基于激光传统自主导航存在对称模式下的唯一定位问题，本发明提供了一种基于自主导航的畜禽信息感知机器人与地图构建方法，用机器人技术完全代替饲养人员在工作环节的功能，实现全自动养殖生产。

为了实现本发明的目的，本发明的具体技术方案如下：

一、一种基于自主导航的畜禽信息感知机器人：

包括四轮小车和安装在四轮小车上的自主导航系统、运动模块、信息采集模块，四轮小车上方设有升降云台；所述的自主导航系统包括用于获取周围环境信息的激光雷达、rgb-d相机、惯性测量单元和里程计以及用于信息处理和控制的主控模块；激光雷达、rgb-d相机、惯性测量单元、里程计和主控模块均固定安装在四轮小车上，激光雷达、rgb-d相机、惯性测量单元、里程计均连接到主控模块；所述的运动模块包括用于控制机器人行进的直流电机组、用于控制升降云台上下升降移动的推杆电机以及控制直流电机组进而推杆电机转速转向的微控制器；直流电机组包括安装于四轮小车底部四角的四个直流电机，四个直流电机的输出轴连接到四轮小车底部四角的四个车轮，推杆电机固定于四轮小车底部上，推杆电机输出端经传动结构连接到升降云台，推杆电机运行带动升降云台上下升降运动；所述的信息采集模块包括用于采集动物行为信息的热成像仪、用于采集环境信息的环境检测传感器模组和用于数据传输的无线传输模块；热成像仪置于升降云台底部的两侧，环境检测传感器模组置于升降云台上。

还包括和电源管理模块，电源管理模块连接到激光雷达、rgb-d相机、惯性测量单元、里程计、主控模块、直流电机组、推杆电机和微控制器。

所述的直流电机组和推杆电机均连接到微控制器，微控制器和主控模块连接，由主控模块接收周围环境信息进而控制直流电机组和推杆电机运行，进而控制四轮小车的移动和升降云台的升降，并使用slam进行地图自主构建和导航。

所述的热成像仪和环境检测传感器模组和无线传输模块电连接，无线传输模块连接到外部无线接收器，外部无线接收器接收热成像仪和环境检测传感器模组采集的环境感知信息进行存储和处理。

所述的环境检测传感器模组包括有多种传感器，包括但不限于声音传感器、温度传感器、湿度传感器、光照强度传感器、硫化氢传感器、氨气传感器、二氧化碳传感器。

二、一种自主导航地图自主构建方法：

步骤s1：控制机器人在室内工作环境移动，同时利用激光雷达、rgb-d相机、惯性测量单元和里程计获取移动过程的周围环境信息，包括障碍物距离信息、图像及深度信息、局部坐标系下的位姿信息和里程计信息，位姿信息包括第一实时全局坐标，里程计信息包括第二实时全局坐标、速度、航向角和车轮的角速度；

步骤s2：主控模块接收周围环境信息进行处理，通过坐标变换得到机器人在世界坐标系下的实时全局坐标；

世界坐标系是以环境为坐标原点的三维立体坐标系；局部坐标系是以四轮小车为坐标原点的三维立体坐标系。

步骤s3：将机器人在世界坐标系下的定位全局坐标、速度、航向角以及角速度作为卡尔曼滤波器的状态向量；所述全局坐标由第一实时全局坐标、第二实时全局坐标和里程计信息处理获得；

步骤s4：根据状态向量构建卡尔曼滤波器的状态模型，根据里程计的观测模型、惯性测量单元的观测模型以及激光雷达的观测模型构建卡尔曼滤波器的观测模型，根据卡尔曼滤波算法对状态模型和观测模型进行求解获得t时刻下状态向量的全局最优解；

步骤s5：结合rgb-d相机采集的图像信息和蒙特卡罗即时定位与地图构建算法，确定步骤s4所述的卡尔曼滤波器的状态模型和观测模型下所述状态向量的全局最优解；

s501：机器人在地图待构建区域移动，通过激光雷达采集的障碍物距离信息判断机器人移动过程中是否转弯和是否有障碍物，通过rgb-d相机采集的图像信息判断是否有拍到特征道路标记；激光雷达、惯性测量单元和里程计对在地图待构建区域采集到的信息数据进行特征匹配得到世界坐标系下的位姿，所述位姿是机器人在世界坐标系下的全局坐标及航向角；

s502：作以下判断处理：

当机器人移动过程中无转弯、无障碍物或rgb-d相机没有拍到特征道路标记时，则卡尔曼滤波器的状态模型的控制向量为世界坐标系下的位姿；

当机器人移动过程中有转弯、有障碍物或rgb-d相机拍到特征道路标记时，则卡尔曼滤波器的状态模型的控制向量为状态向量的最优解；

步骤s6：对卡尔曼滤波器的状态模型和观测模型进行迭代求解得到定位位置，进而构建获得全局地图。

所述的步骤s4，具体为：

s401：t时刻，状态向量xc(t)构建为xc(t)＝[xt，yt，θt，vt，ωt]^t，其中，xt、yt为机器人在世界坐标系下的定位全局坐标，θt为航向角，vt为速度，ωt为角速度，t为矩阵转置；

s402：按照以下构建卡尔曼滤波器状态模型：

xc(t+1)＝f(xc(t+1))+wt

其中，xc(t)表示t时刻的状态向量，f(xc(t))为t时刻的状态向量xc(t)的非线性状态转移函数，wt为卡尔曼滤波器过程噪声，δt为相邻两个时刻之间的时间间隔；

s403：将卡尔曼滤波器拆分为各自并列独立的第一子滤波器和第二子滤波器的两部分，其中：

第一子滤波器的观测模型为zt+1＝h1xc(t)+w1(t)，具体为：

其中，zlas为激光雷达的观测模型，zimu为惯性测量单元的观测模型，w1(t)为激光雷达与惯性测量单元的噪声，h1为第一子滤波器的观测矩阵；

第二子滤波器的观测模型为z2(t+1)＝h2xc(t)+w2(t)，具体的：

其中，zodom为里程计的观测模型，w2(t)为里程计与惯性测量单元的噪声，h2为第二子滤波器的观测矩阵；

s404：采用以下公式将滤波器的过程噪声wt的协方差q(t)、滤波器的估计误差协方差p(t)进行处理并分配到第一子滤波器和第二子滤波器，具体为：

q1(t′)＝α1^-1q(t)

p1(t′)＝(1-α1^-1)p(t)

q2(t′)＝α2^-1q(t)

p2(t′)＝(1-α2^-1)p(t)

其中，q1(t’)为各子滤波器的t时刻量测噪声的协方差，p1(t’)为各子滤波器t时刻的估计误差协方差，α1表示第一子滤波器权值的分配系数；q2(t’)为各子滤波器的t时刻量测噪声的协方差，p2(t’)为各子滤波器t时刻的估计误差协方差，α2表示第二子滤波器权值的分配系数；为t时刻状态向量的全局最优解，为各子滤波器t时刻状态向量的全局最优解。

所述的步骤s6，具体为：

s601：将运动观测模型转换为似然函数；

s602：将地图待构建区域均分为多个栅格，采用激光雷达扫描地图待构建区域，将存在障碍物的栅格设置为1，将没有障碍物的栅格设置为0，从而得到局部栅格地图，并作为初始全局地图；

s603：根据蒙特卡洛算法建立粒子，以粒子的位置作为机器人可能的定位位置，将里程计和惯性测量单元获得的实时全局坐标进行加权融合获得机器人新定位位置，具体为：

pⁱ＝podam·θδt+pimu·(1-θδt′)

其中，p是加权融合后的定位位置，podam是里程计获得的第二实时全局坐标，pimu是惯性测量单元获得的第一实时全局坐标，θδt′是里程计权重，δt′是定位持续时间；γ是里程计获得的第二实时全局坐标到达稳定的定位持续时间，n表示时间指数参数，由实际情况而定，一般取3；

s604：采用均值为0、方差为σ²的高斯分布描述粒子权重更新方法，更新蒙特卡罗算法的粒子权重，更新后的粒子权重为：

其中，是第i个粒子时刻k的平面位置；e表示自然常数，表示第i个粒子初始的平面位置，表示第i个粒子时刻k的权重，k表示时刻；

然后对更新后的粒子权重进行归一化处理；

s605：根据更新后的粒子权重计算机器人的当前定位位置：

其中，n表示粒子的总数，pⁱ是加权融合后的第i个粒子的位置；

s606：根据更新后的粒子权重，舍弃权重较小的粒子，包括权值较大的粒子，具体如下：

s6061：对更新后的所有粒子的权重采用多项式重采样，构建离散累积分布函数：

其中，f(i)表示第i个粒子的累计权重；

s6062：在[0，1]上产生服从均匀分布的随机数集合{uj}，uj表示上述产生的随机数集合，j表示上述集合中随机产生第j个随机数，然后进行以下判断：

当累计权重f(i)≤uj时，则认为粒子的权重较小；

当累计权重f(i)＞uj时，则认为粒子的权重较大，复制当前粒子作为新粒子，且权重设置为1/n；

s6063：重复步骤s6062的多项式重采样进行n次，生成n个新粒子，完成粒子更新，以最终更新得到的粒子的位置作为机器人在世界坐标系上的定位位置。

本发明具体实施中还融合特征路标图像信息，如果rgb-d相机呈现图像中没有畜禽的笼具信息，且当前定位全局坐标与激光扫到的边缘障碍距离小于阈值，导航系统向运动模块发布转弯信号，然后前行至下一路口位置，再次发布转弯信号，直至导航模式关闭。

本发明还包括自主导航系统的路径规划过程，具体包括：

步骤1：输入环境地图，通过蒙特卡罗算法对机器人的位姿进行估计，匹配机器人在栅格地图与真实工作环境的位姿；

步骤2：输入导航目标的位姿；

步骤3：自主导航主控模块整合上述信息，利用迪杰斯特拉算法进行全局路径规划，获得最优路径；

步骤4：自主导航主控模块进行本地实时规划，将机器人行进的线速度、角速度的控制信号传输给运动模块，完成对机器人的电机控制，实现机器人的自主导航移动。

所述步骤1的输入地图可以是机器人自主构建的环境地图或既有的工作环境地图；若是既有的工作环境地图，则省略地图构建部分，在完成地图格式转换后直接进行路线规划。

步骤3中的全局路径规划根据给定目标位置和全局地图进行总体路径的规划，计算机器人到目标位置的最优路线，作为机器人的全局路线。

所述步骤4中的本地实时规划中，利用动态窗口法(dynamicwindowapproach)根据地图信息和附近可能出现的障碍物规划更改每个周期的行进路线，综合评价时间、碰撞等条件选取最优路线，并计算行进周期内的线速度和角速度，实现实时避障。

本发明在激光雷达与里程计的基础上加入惯性测量单元定位信息和特征图像信息，以协同的方式发挥各种定位的优点和互补性，从而得到机器人的状态向量的最优解；再利用特征图像信息处理的快速性和精确性的特点，可有效解决畜禽舍内对称模式下的唯一定位问题，形成精确度较高的位姿预测信息，同时降低蒙特卡罗算法即时定位与构图的粒子数量，由此能够进一步提高机器人(低功率处理器)的定位精度，保证畜禽信息感知机器人在对称环境的定位效果，完成地图构建，满足畜禽舍环境的应用需求。

本发明通过激光、里程计、惯性测量单元和图像信息有效融合，可以实现地图自主构建和路径规划，实现在工作环境下的自主导航，并利用检测传感器模组和无线传输模块，实时采集并传输多种环境信息、动物行为健康等信息，解决了现有畜禽舍巡检方式效率低、成本高且对饲养人员依赖严重的问题。

同时本发明融合激光、里程计、惯性测量单元和图像信息，以协同的方式发挥各种定位的优点和互补性，结合特征路标识别精确和快速的特点，形成精确度较高的位姿预测信息，从而降低蒙特卡罗算法即时定位与地图构建方法中的粒子数量，能够极大地提高机器人(低功率处理器)的定位精度，从而进一步提高机器人在畜禽舍内的定位精度。

本发明与现有技术相比，其显著优点为：

1)本发明结合机器人和自主导航技术，提供了一种基于自主导航的畜禽信息感知机器人与方法，替代饲养员进行日常巡检工作，实现无人自主对结构化畜禽养殖环境进行环境信息的自动检测和动物行为的自动采集。

2)结合卡尔曼滤波算法，根据里程计的观测模型、惯性测量单元的观测模型以及激光雷达的观测模型构建卡尔曼滤波器的观测模型，在传统的激光雷达与里程计的基础上加入惯性测量单元定位信息和特征图像信息，以协同的方式发挥各种定位的优点和互补性，从而得到机器人的状态向量的最优解。

3)再结合特征路标识别精确和快速的特点，形成精确度较高的位姿预测信息，从而降低蒙特卡罗算法即时定位与地图构建方法中的粒子数量，能够极大地提高机器人(低功率处理器)的定位精度，保证畜禽信息感知机器人在对称环境的定位效果，完成地图构建，满足畜禽舍环境的应用需求。

4)首次将ros(robotoperationsystem)作为机器人操作系统应用到畜禽养殖行业中，以slam(simultaneouslocalizationandmapping，即时定位与地图构建)技术为核心，集成卡尔曼滤波、粒子滤波算法，实现机器人的路径规划，便于后续对产品进行二次开发。

本发明克服现存技术及应用对饲养人员的严重依赖性，旨在不改变养殖环境本身结构的情况下，结合机器人和自主导航技术，用基于自主导航的畜禽信息感知/采集机器人来替代饲养员进行日常巡检工作，实现对养殖环境进行环境信息和动物行为(健康状态)自动检测，防止人进入畜禽舍后造成的不良影响，为家禽养殖和畜牧业的可持续性、健康发展提供了技术保障，具有效率高、经济效益高、适用性广等优点，具有极高的行业价值和应用价值。

附图说明

图1为本发明提供的装置结构图；

图2为本发明自主导航系统的方法整体流程图；

图3为本发明自主导航系统地图构建流程图；

图4为本发明自主导航系统的路径规划方法流程图；

图5为本发明自主导航系统的地图构建结果图。

图中：直流电机组1、微控制器2、四轮小车3、推杆电机4、惯性测量单元5、热成像仪6、升降云台7、环境检测传感器模组8、rgb-d相机9、激光雷达10、无线传输模块11、主控模块12、电源管理模块13。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明的技术方案、原理和特征进行描述,所举实例只用于解释本发明,并非用于限定本发明的范围。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的其他实施例，都属于本发明保护的范围。

如图1所示，包括四轮小车3和安装在四轮小车3上的自主导航系统、运动模块、信息采集模块，四轮小车3上方设有升降云台7。

如图1所示，自主导航系统包括用于获取周围环境信息的激光雷达10、rgb-d相机9、惯性测量单元5和里程计以及用于信息处理和控制的主控模块12；激光雷达10、rgb-d相机9、惯性测量单元5、里程计和主控模块12均固定安装在四轮小车3上，激光雷达10、rgb-d相机9、惯性测量单元5、里程计均连接到主控模块12；rgb-d相机9和激光雷达10位于四轮小车3前部，rgb-d相机9朝向正前方。

如图1所示，运动模块包括用于控制机器人行进的直流电机组1、用于控制升降云台7上下升降移动的推杆电机4以及控制直流电机组1进而推杆电机4转速转向的微控制器2；直流电机组1包括安装于四轮小车3底部四角的四个直流电机，四个直流电机的输出轴连接到四轮小车3底部四角的四个车轮，推杆电机4固定于四轮小车3底部上，推杆电机4输出端经传动结构连接到升降云台7，推杆电机4运行带动升降云台7上下升降运动；直流电机组1和推杆电机4均连接到微控制器2，微控制器2和主控模块12连接，由主控模块12接收周围环境信息进而控制直流电机组1和推杆电机4运行，进而控制四轮小车3的移动和升降云台7的升降。

机器人主体部分是具有升降云台7的四轮小车3，四轮小车3上安装了具有升降功能的升降云台7，升降云台7上安装有热成像仪6和环境检测传感器模组8，可根据任务需求采集不同位置的信息，但不限于仅实现信息感知功能。

如图1所示，信息采集模块包括用于采集动物行为信息的热成像仪6、用于采集环境信息的环境检测传感器模组8和用于数据传输的无线传输模块11；热成像仪6和环境检测传感器模组8和无线传输模块11电连接，无线传输模块11连接到外部无线接收器；热成像仪6置于升降云台7底部的两侧，环境检测传感器模组8置于升降云台7上。信息采集模块的热成像仪采用对称的安装方式，可实现同时收集检测畜禽舍过道两侧不同高度的动物行为状态。

具体实施还包括和电源管理模块13，电源管理模块13连接到激光雷达10、rgb-d相机9、惯性测量单元5、里程计、主控模块12、直流电机组1、推杆电机4和微控制器2，电源管理模块13为机器人各部件提供正常工作所需的电力。

升降云台7带动热成像仪6和环境检测传感器模组8上下升降对高度不同的畜禽笼具进行感知采集所需环境信息。

激光雷达10用于实时测量机器人与周围环境障碍物的距离，rgb-d相机9用于获取机器人周围环境的rgb图像和深度信息，惯性测量单元5用于获取机器人的位姿信息，里程计用于获取机器人的里程计信息。主控模块12基于ros平台并且结合卡尔曼滤波与粒子滤波算法实现地图自主构建与路径规划。

环境检测传感器模组8包括有多种传感器，包括但不限于声音传感器、温度传感器、湿度传感器、光照强度传感器、硫化氢传感器、氨气传感器、二氧化碳传感器。

如图2所示，本发明具体实施例及其实施过程如下：

自主导航分为地图自主构建和路径规划两个部分。

地图自主构建过程包括以下步骤：

步骤s1：通过机器人操作系统ros平台控制机器人在室内工作环境移动，同时利用激光雷达、rgb-d相机、惯性测量单元和里程计获取移动过程的周围环境信息，包括障碍物距离信息、图像及深度信息、局部坐标系下的位姿信息和里程计信息，位姿信息包括第一实时全局坐标，里程计信息包括第二实时全局坐标、速度、航向角和车轮的角速度；

步骤s2：主控模块接收周围环境信息进行处理，通过坐标变换得到机器人在世界坐标系下的实时全局坐标；

步骤s3：将机器人在世界坐标系下的定位全局坐标、速度、航向角以及角速度作为卡尔曼滤波器的状态向量；全局坐标由第一实时全局坐标、第二实时全局坐标和里程计信息处理获得；

步骤s4：根据状态向量构建卡尔曼滤波器的状态模型，根据里程计的观测模型、惯性测量单元的观测模型以及激光雷达的观测模型构建卡尔曼滤波器的观测模型，根据卡尔曼滤波算法对状态模型和观测模型进行求解获得状态向量的最优解；

s401：t时刻，状态向量xc(t)构建为xc(t)＝[xt，yt，θt，vt，ωt]^t，其中，xt、yt为机器人在世界坐标系下的定位全局坐标，θt为航向角，vt为速度，ωt为角速度，t为矩阵转置；

s402：按照以下构建卡尔曼滤波器状态模型：

xc(t+1)＝f(xc(t+1))+wt

其中，xc(t)表示t时刻的状态向量，f(xc(t))为t时刻的状态向量xc(t)的非线性状态转移函数，wt为卡尔曼滤波器过程噪声，δt为相邻两个时刻之间的时间间隔；

s403：将卡尔曼滤波器拆分为各自并列独立的第一子滤波器和第二子滤波器的两部分，其中：

第一子滤波器的观测模型为zt+1＝h1xc(t)+w1(t)，具体为：

其中，zlas为激光雷达的观测模型，zimu为惯性测量单元的观测模型，w1(t)为激光雷达与惯性测量单元的噪声，h1为第一子滤波器的观测矩阵；

第二子滤波器的观测模型为z2(t+1)＝h2xc(t)+w2(t)，具体的：

其中，zodom为里程计的观测模型，w2(t)为里程计与惯性测量单元的噪声，h2为第二子滤波器的观测矩阵；

s404：采用以下公式将滤波器的过程噪声wt的协方差q(t)、滤波器的估计误差协方差p(t)进行处理并分配到第一子滤波器和第二子滤波器，具体为：

q1(t′)＝α1^-1q(t)

p1(t′)＝(1-α1^-1)p(t)

q2(t′)＝α2^-1q(t)

p2(t′)＝(1-α2^-1)p(t)

步骤s5：构建蒙特卡罗算法的运动状态转移模型与运动观测模型

s501：结合rgb-d相机采集的图像信息和蒙特卡罗即时定位与地图构建算法，确定步骤s4所述的卡尔曼滤波器的状态模型和观测模型下所述状态向量的全局最优解；

s502：机器人在地图待构建区域移动，通过激光雷达采集的障碍物距离信息判断机器人移动过程中是否转弯和是否有障碍物，通过rgb-d相机采集的图像信息判断是否有拍到特征道路标记；激光雷达、惯性测量单元和里程计对在地图待构建区域采集到的信息数据进行特征匹配得到世界坐标系下的位姿。

s503：作以下判断处理：

当机器人移动过程中无转弯、无障碍物或rgb-d相机没有拍到特征道路标记时，则卡尔曼滤波器的状态模型的控制向量为世界坐标系下的位姿；

当机器人移动过程中有转弯、有障碍物或rgb-d相机拍到特征道路标记时，则卡尔曼滤波器的状态模型的控制向量为状态向量的最优解，即状态向量中包含的五个值；

步骤s6：对卡尔曼滤波器的状态模型和观测模型采用后续的粒子迭代更新处理进行迭代求解得到定位位置，进而构建获得全局地图。

s601：将运动观测模型转换为似然函数；

s602：将地图待构建区域均分为多个栅格，采用激光雷达扫描地图待构建区域，将存在障碍物的栅格设置为1，将没有障碍物的栅格设置为0，从而得到局部栅格地图，并作为初始全局地图；

s603：根据蒙特卡洛算法建立粒子，以粒子的位置作为机器人可能的定位位置，将里程计和惯性测量单元获得的实时全局坐标进行加权融合获得机器人新定位位置，具体为：

pⁱ＝podam·θδt+pimu·(1-θδt′)

其中，p是加权融合后的定位位置，podam是里程计获得的第二实时全局坐标，pimu是惯性测量单元获得的第一实时全局坐标，θδt′是里程计权重，δt′是定位持续时间；γ是里程计获得的第二实时全局坐标到达稳定的定位持续时间，n表示时间指数参数，由实际情况而定，一般取3；

s604：采用均值为0、方差为σ²的高斯分布描述粒子权重更新方法，更新蒙特卡罗算法的粒子权重，更新后的粒子权重为：

其中，是第i个粒子时刻k的位置；e表示自然常数，表示第i个粒子初始位置，表示第i个粒子时刻k的权重，k表示时刻；

然后对更新后的粒子权重进行归一化处理；

s605：根据更新后的粒子权重计算机器人的当前定位位置：

其中，n表示粒子的总数，pⁱ是加权融合后的第i个粒子的位置；

s606：根据更新后的粒子权重，舍弃权重较小的粒子，包括权值较大的粒子，具体如下：

s6061：对更新后的所有粒子的权重采用多项式重采样，构建离散累积分布函数：

其中，f(i)表示第i个粒子的累计权重；

s6062：在[0，1]上产生服从均匀分布的随机数集合(uj}，uj表示上述产生的随机数集合，j表示上述集合中随机产生第j个随机数，然后进行以下判断：

当累计权重f(i)≤uj时，则认为粒子的权重较小；

当累计权重f(i)＞uj时，则认为粒子的权重较大，复制当前粒子作为新粒子，且权重设置为1/n；

s6063：重复步骤s6062的多项式重采样进行n次，生成n个新粒子，完成粒子更新，以最终更新得到的粒子的位置作为机器人在世界坐标系上的定位位置。

具体实施中还融合特征路标图像信息，如果rgb-d相机呈现图像中没有畜禽的笼具信息，且当前定位全局坐标与激光扫到的边缘障碍距离小于阈值，导航系统向运动模块发布转弯信号，然后前行至下一路口位置，再次发布转弯信号，直至导航模式关闭。

如图3所示自主导航系统的路径规划过程包括以下步骤：

步骤1：输入环境地图，输入的环境地图为上述步骤机器人自主构建的全局地图，且将全局地图栅格化处理。通过蒙特卡罗算法对机器人的位姿进行估计，匹配机器人在占据栅格后的全局地图与真实工作环境的位姿；

步骤2：输入导航目标的位姿；

步骤3：自主导航主控模块整合上述信息，利用迪杰斯特拉算法进行全局路径规划，获得最优路径。全局路径规划根据给定目标位置和全局地图进行总体路径的规划，计算机器人到目标位置的最优路线，作为机器人的全局路线。

步骤4：自主导航主控模块进行本地实时规划，将机器人行进的线速度、角速度的控制信号传输给运动模块，完成对机器人的电机控制，实现机器人的自主导航移动。

本地实时规划中，利用动态窗口法dynamicwindowapproach根据地图信息和附近可能出现的障碍物规划更改每个周期的行进路线，综合评价时间、碰撞等条件选取最优路线，并计算行进周期内的线速度和角速度，实现实时避障。