【技术实现步骤摘要】
一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法
[0001]本专利技术涉及计算机视觉技术、激光雷达建模领域，特别涉及一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法。
技术介绍
[0002]随着科技的发展，人工智能技术已经开始渗入畜牧行业，然而国内畜牧业的智能化程度还处于低级阶段，猪舍、牛舍、羊舍的清洗大部分仍是由人工完成的，耗费人力，效率较低；畜牧智能设备普及率低、多依赖于进口且成本高；现有清洗设备普适率低，无法达全方位清洗。因此高效率、成本低、适应性广的畜牧清洗自动驾驶车开发刻不容缓。而自动驾驶加入畜牧业清洗将会成为一个新的技术研究热点，带动畜牧业加快发展。
[0003]目前市场上推出的畜牧自动清洗车大多成本高，且无法适用不同环境得畜牧舍，清洗不彻底。因此性价比与推广性达不到市场要求。
技术实现思路
[0004]为了克服上述现有技术的不足，本专利技术的目的在于提供一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，利用固态面阵激光雷达对整个畜牧舍进行建模，得到3D畜牧舍模型，根据模型规划可清洗整个畜牧舍路径，行驶时根据激光雷达和机器视觉检测出清洗目标(栅栏、挡板)的三维目标信息，匹配畜牧舍清洗目标三维模型信息，然后控制车速及清洗喷头的纵向摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
[0005]为了实现上述功能，本专利技术采用的技术方案是：
[0006]一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，包括以下步骤；
[0007]步骤一，使用激
【技术保护点】
【技术特征摘要】
1.一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，包括以下步骤；步骤一，使用激光雷达采集畜牧舍点云数据；步骤二，使用体素化网格下采样的算法处理点云数据，对点云数据进行去噪滤波，获取较为干净的畜牧舍点云数据；步骤三：使用NDT和ICP融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3D模型；步骤四：使用Cartographer算法构建畜牧舍3D模型，利用rosbag录制激光雷达的3D数据和IMU的数据，采用Cartographer算法根据录制的数据构建模型，获取畜牧舍3D模型；步骤五：根据步骤四所得畜牧舍3D模型，确定畜牧舍入口和出口坐标，采用A*算法规划出清洗畜牧舍挡板、围栏的可通行路径；步骤六：制作棋盘标定板，采用张正友算法对CMOS摄像头进行标定，获取CMOS摄像头参数矩阵；步骤七：使用车上固定的CMOS摄像头和激光雷达采集图像、点云，利用solvePnP算法通过步骤六得到的参数矩阵，计算CMOS摄像头到激光雷达的旋转矩阵和平移矩阵，最后通过获取的旋转矩阵将点云数据投影到二维图像上，融合摄像头图像和激光雷达点云数据，获取到三维图像；步骤八：对步骤七获取的一组三维图像进行标定，采用PointRCNN算法对标定的数据集进行训练，得到专用于检测畜牧舍目标的模型，当畜牧清洗自动驾驶车沿规划的路径行驶时，实时框取检测目标的三维框；步骤九：通过步骤八获取的目标三维检测模型实时检测并识别前方挡板、工作人员等障碍物，框取检测物三维框，获取其三维信息，采用人工势场法通过三维信息规划处可通行局部路径，主控器控制小车的左转、右转、前进、后退、停止，实现行驶时的局部路径规划，保证小车无碰撞清洗；步骤十：根据步骤八获取的清洗目标三维检测模型实时获取清洗对象的三维信息，匹配步骤三所得清洗对象3D模型，获取挡板、栅栏的准确清洗范围，通过PLC控制摆动电机和往复电机实现对清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。2.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤一中采集方法具体为：将固态面阵激光雷达固定在畜牧清洗自动驾驶车上，车沿畜牧舍可通行路径行驶，采集畜牧舍内点云数据，并保存至数据bag中。3.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤二中使用体素化网格下采样处理步骤一获取的数据bag中的每一帧点云数据，对点云数据进行去噪滤波；体素化网格下采样算法步骤为：根据点云数据创建一个最小三维体素栅格，然后计算出需要划分的小立方栅格的边长L，根据L的大小将三维体素栅格分解成m
×
n
×
l个小栅格，栅格划分完毕后将点云数据放到相应的小栅格中，同时删除那些不包含数据点的小栅格，在每个小栅格中，将离小栅格重心最近的数据点保留下来，删除其余的数据点，以此对点云数据进行去噪滤波，获取较为干净的畜牧舍点云数据。其计算步骤如下：
(1)小立方栅格的边长L的确定，小立方栅格的边长与邻近点个数k成正比，与点云的平均密度成反比，当点云的平均密度小时，表示在固定空间内的点云数量少，那么应将L取大些来提高k邻近搜索的范围，以保证有足够多点云进行k近邻的计算；当点云的平均密度大时，则表示在固定空间内的点云数量较多，应将L取小些，以保证在最适当的范围内搜索，降低搜索的时间；小立方体栅格边长为如式1
‑
1：式中:α是比例因子，用来调节小立方体栅格的边长；s是比例系数；g是小栅格中点云数据的数目；单位小栅格中包含的点云数据个数如式1
‑
2：n＝N/V
ꢀꢀꢀꢀ
(1
‑
2)式中:N表示点云数据中点的总数；V表示三维体素栅格的体积；三维体素栅格的体积如式1
‑
3：V＝L
x
L
y
L
z
ꢀꢀꢀꢀ
(1
‑
3)式中:L
x
表示点云X轴方向最大范围；L
x
表示点云Y轴方向最大范围；L
x
表示点云Z轴方向最大范围，适当的将长方体向外扩大，增加距离λ对其修正，所以，三维体素栅格三边的长度是：将式(1
‑
2)、(1
‑
3)代入式(1
‑
1)可得公式1
‑
5：(2)将点云数据划分到小立方体栅格中，根据小立方体栅格的边长L，将点云数据划分为m
×
n
×
l个小立方体栅格，其中，m＝ceil(L
x
/l)，n＝ceil(L
y
/l)，l＝ceil(L
z
/l)，ceil(x)为取整函数，表示不小于的最小整数。对于任一点pi，其所属小立方体栅格号如式1
‑
6：在整个栅格编码中，点P
i
的栅格编码为如果转换为一维编码的话，点P
i
的栅格编码如式1
‑
7所示：根据式(1
‑
6)或式(1
‑
7)点云数据模型的每个数据点的栅格编码，且将编码放进哈希链表，建立点云数据间的空间拓扑关系后，确定出每个数据点P
i
的邻近点，数据点P
i
的邻近点是在它本身所在的小栅格和相邻小栅格中的数据点，而k 邻域的确定是计算这些数据点到点P
i
的距离，并将按距离的远近，取出离P
i
点最近的前k个数据点，存入到哈希链表中，作为
数据点P
i
的k邻域点，k的取值，要综合考虑物体表面的凹凸性和点云数据模型的密度，并且要确保重建得到的曲面在每个数据点的邻近范围内是单凸性或者单凹性的，故k取值范围一般为8～20；(3)散乱点云法向量的估算，利用最小二乘平面法来估计数据点的法向量，假设物体的采样表面是足够光滑的，则采样得到的物体表面的数据点就可以用最小二乘平面法来实现曲面拟合，根据点云数据模型中的每个数据点P
i
及其k近邻点，计算出每个数据点P
i
及其k近邻点的最小二乘局部平面H，公式如1
‑
8所示：式中:θ表示高斯权重；n表示最小二乘局部平面H的法向量，满足n2＝1；d表示最小二乘局部平面H与坐标原点两者之间的距离；最小二乘局部平面平面H的法向量可以通过主成分分析得到，数据点P
i
经过它的k邻域，如式1
‑
9所示：对式(1
‑
9)分解，得到协方差矩阵M的3特征值λ0，λ1，λ2，设0≤λ0≤λ1≤λ2，对应的3个特征向量分别是v0，v1，v2，则最小特征向量v0可以近似表示数据点P
i
的法向量，协方差矩阵M如式1
‑
10所示：(4)代表点的选取及下采样实现，根据求出的点云数据模型中每个点P
i
的邻域和点云数据模型中每个数据点P
i
的法向量，求出每个数据点P
i
的法向量与其邻近点法向之间的夹角，若法向量之间的夹角越大，则说明数据点P
i
在其k近邻附近的曲率变化比较大，存在尖锐的几何特征，相反，若法向量之间的夹角越小，则说明数据点P
i
在其k近邻附近的曲率变化比较小，存在稀疏的几何特征，预先设定一个阈值，当数据点P
i
的法向量与其邻近点法向量之间的夹角大于阈值，则将该数据点视为特征点，调整边长比例因子α，使得立方体栅格边长变小，每个栅格点云数量变少，这样保留较多特征数据点，夹角小于阈值的点作为非特征点，调整边长比例因子α，使得立方体栅格变大，每个栅格点云数量变多，保留较少非特征数据点，每个小立方体栅格中，三维栅格中心集重心如式1
‑
11所示：
式中g为小立方体栅格中点云数据个数。选取最靠近重心数据点保留下来，代表这个小立方体栅格所有数据点，实现点云下采样。完成对步骤一采集的点云数据进行滤波去噪，获取较为干净的畜牧舍点云数据。4.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤三中使用NDT和ICP融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3D模型；首先根据步骤二所得点云数据得到网格参数大小，配准距离误差数据，其次进行NDT算法粗配准，，并对配准后的点云进行30％的抽稀，具体算法为：(1)将点云数据划分为数个大小均匀的三维立方体，每个立方体中都包含有一定数量的点；(2)点云在立方体中服从高斯分布，因此点云数据的高斯分布参数，如式1
‑
12，1
‑
13：13：式中u表示均值向量，C表示协方差矩阵，n为立方体中总点数，xi为立方体中的一点，T表示转换矩阵；(3)当一点落入小方格内部时，就根据内部的u和C对该点进行打分s，如式1
‑
14：(4)最小化(3)中的目标函数即可得到转换矩阵T，如式1
‑
15：(5)利用求得的变换矩阵对获取点云进行旋转变换，融合到相邻的点云数据中，并对配准后的点云进行30％的抽稀，得到一个粗配准点云结果；然后进行KD树索引构建，搜索NDT粗配准点云的最近点，为ICP精细配准提供配准最近点，具体算法为：(1)确定分割阈值：对所有的数据分别计算各个维度上数据的方差，选方差最大值作为
所在维的分割域值；(2)确定根节点：按照分割域值对三维数据进行排序，选中间点作为KD树的根节点；(3)确定左右子树空间：确定根节点后，使得所有小于根节点的点出现在左子树，剩余的点出现在右子树；(4)分别对左右子树的点重复(2)和(3)，直至每个子空间都只有一个点，当搜索相邻点时，首先确定搜索路径上最近点，然后回溯，检查有无最近点，将查询点和最近点的距离与最近点和分割平面的距离进行对比，前者大则进入该点的子空间，否则继续回溯直至前者大于后者，最终确定最近点；最后通过ICP算法进行精细配准，得到畜牧舍清洗对象的3D模型，具体算法为：(1)在点云B中取一点b
i
∈B；(2)在点云A中找出与之对应的点ai∈A使得ai－bi＝min；(3)计算出旋转矩阵R和平移矩阵t，使得误差函数最小；(4)使用上步得出的旋转矩阵R和平移矩阵t对bi进行变换，得到新的点集，如式1
‑
16:b
′
i
＝Rb
i
+t,b
i
∈B
ꢀꢀꢀꢀ
(1
‑
16)(5)计算b
′
i
与点集A中与之对应的点ai的平均距离，如式1
‑
17:b
′
i
＝Rb
i
+t,b
i
∈B
ꢀꢀꢀꢀ
(1
‑
17)(6)若d小于设定的阈值或大于设定的迭代次数，则停止计算，否则返回步骤(2)，直到满足收敛条件为止。5.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤四中使用改进的Cartographer算法通过步骤二获得的处理过的点云数据进行配准建模，获取畜牧舍3D模型；首先采用前端Local累加式建图，其主要步骤为：(1)对一帧点云的每个点数据，筛出小于指定距离的点，并逐个记录为returns；对于大于指定距离的点，将该方向上一定距离的某点记入misses点集。此处可将多帧数据一并存入并在后续工作中作一帧处理，默认仅累计一帧的数据；(2)对输入的点云数据经过修剪筛去了z轴阈值之外的点。之后分别对returns点集和misses点集进行体素滤波。每个体素内仅保留一个点；(3)从姿态推算器中获取当前点云帧的姿态初值，并优化点云帧的位姿，匹配完成后把点云帧插入子图；(4)将当前帧与较活跃的子图进行匹配以优化当前帧位姿，首先通过real
‑
timeCSM对激光帧数据在submap的栅格地图上进行一次粗匹配，产生一个大体较好的位姿值并返回一个得分数；...
【专利技术属性】
技术研发人员：李颀，李煜哲，叶小敏，郭梦媛，冯文斌，马凯，张冉，
申请(专利权)人：陕西科技大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。