[发明专利]一种基于自主导航的畜禽信息感知机器人与地图构建方法

权利要求书

1.一种基于自主导航的畜禽信息感知机器人与地图构建方法，其特征在于：方法包括以下步骤：

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

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

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

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

所述的步骤S4，具体为：

S401：t时刻，状态向量X_c(t)构建为X_c(t)＝[x_t，y_t，θ_t，v_t，ω_t]^T，其中，x_t、y_t为机器人在世界坐标系下的定位全局坐标，θ_t为航向角，v_t为速度，ω_t为角速度，T为矩阵转置；

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

X_c(t+1)＝f(X_c(t+1))+W_t

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

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

第一子滤波器的观测模型为Z_t+1＝h₁X_c(t)+W_1(t)，具体为：

其中，Z_Las为激光雷达的观测模型，Z_IMU为惯性测量单元的观测模型，W_l(t)为激光雷达与惯性测量单元的噪声，h₁为第一子滤波器的观测矩阵；

第二子滤波器的观测模型为Z_2(t+1)＝h₂X_c(t)+W_2(t)，具体的：

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

S404：采用以下公式将卡尔曼滤波器的过程噪声W_t的协方差Q_(t)、卡尔曼滤波器的估计误差协方差P_(t)进行处理并分配到第一子滤波器和第二子滤波器，具体为：

Q_1(t′)＝α₁^-1Q_(t)

P_1(t′)＝(1-α₁^-1)P_(t)

Q_2(t′)＝α₂^-1Q_(t)

P_2(t′)＝(1-α₂^-1)P_(t)

其中，Q_1(t′)为第一子滤波器的t时刻量测噪声的协方差，P_1(t′)为第一子滤波器t时刻的估计误差协方差，α₁表示第一子滤波器权值的分配系数；Q_2(t′)为第二子滤波器的t时刻量测噪声的协方差，P_2(t′)为第二子滤波器t时刻的估计误差协方差，α₂表示第二子滤波器权值的分配系数；为t时刻状态向量X_c(t)的全局最优解，为各子滤波器t时刻状态向量的全局最优解；

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

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

S502：作以下判断处理：

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

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

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

所述的步骤S6，具体为：

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

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

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

P¹＝Po_dam·θ_Δt+PI_MU·(1-θ_Δt′)

其中，Pⁱ是加权融合后的第i个粒子的位置，P_odam是里程计获得的第二实时全局坐标，P_IMU是惯性测量单元获得的第一实时全局坐标，θ_Δt′是里程计权重，Δt′是定位持续时间；γ是里程计获得的第二实时全局坐标到达稳定的定位持续时间，n′表示时间指数参数，由实际情况而定；

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

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

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

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

其中，n表示粒子的总数；

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

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

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

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

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

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

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