一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法与流程

1.本发明涉及计算机视觉技术、激光雷达建模领域，特别涉及一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法。


背景技术：

2.随着科技的发展，人工智能技术已经开始渗入畜牧行业，然而国内畜牧业的智能化程度还处于低级阶段，猪舍、牛舍、羊舍的清洗大部分仍是由人工完成的，耗费人力，效率较低；畜牧智能设备普及率低、多依赖于进口且成本高；现有清洗设备普适率低，无法达全方位清洗。因此高效率、成本低、适应性广的畜牧清洗自动驾驶车开发刻不容缓。而自动驾驶加入畜牧业清洗将会成为一个新的技术研究热点，带动畜牧业加快发展。
3.目前市场上推出的畜牧自动清洗车大多成本高，且无法适用不同环境得畜牧舍，清洗不彻底。因此性价比与推广性达不到市场要求。


技术实现要素：

4.为了克服上述现有技术的不足，本发明的目的在于提供一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，利用固态面阵激光雷达对整个畜牧舍进行建模，得到3d畜牧舍模型，根据模型规划可清洗整个畜牧舍路径，行驶时根据激光雷达和机器视觉检测出清洗目标(栅栏、挡板)的三维目标信息，匹配畜牧舍清洗目标三维模型信息，然后控制车速及清洗喷头的纵向摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
5.为了实现上述功能，本发明采用的技术方案是：
6.一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，包括以下步骤；
7.步骤一，使用激光雷达采集畜牧舍点云数据；
8.步骤二，使用体素化网格下采样的算法处理点云数据，对点云数据进行去噪滤波，获取较为干净的畜牧舍点云数据；
9.步骤三：使用ndt和icp融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3d模型；
10.步骤四：使用cartographer算法构建畜牧舍3d模型，利用rosbag录制激光雷达的3d数据和imu的数据，采用cartographer算法根据录制的数据构建模型，获取畜牧舍3d模型；
11.步骤五：根据步骤四所得畜牧舍3d模型，确定畜牧舍入口和出口坐标，采用a*算法规划出清洗畜牧舍挡板、围栏的可通行路径；
12.步骤六：制作棋盘标定板，采用张正友算法对cmos摄像头进行标定，获取cmos摄像头参数矩阵；
13.步骤七：使用车上固定的cmos摄像头和激光雷达采集图像、点云，利用 solvepnp算法通过步骤六得到的参数矩阵，计算cmos摄像头到激光雷达的旋转矩阵和平移矩阵，最后通过获取的旋转矩阵将点云数据投影到二维图像上，融合摄像头图像和激光雷达点云数
据，获取到三维图像；
14.步骤八：对步骤七获取的一组三维图像进行标定，采用pointrcnn算法对标定的数据集进行训练，得到专用于检测畜牧舍目标的模型，当畜牧清洗自动驾驶车沿规划的路径行驶时，实时框取检测目标的三维框；
15.步骤九：通过步骤八获取的目标三维检测模型实时检测并识别前方挡板、工作人员等障碍物，框取检测物三维框，获取其三维信息(位置、大小、距离)，采用人工势场法通过三维信息规划处可通行局部路径，主控器控制小车的左转、右转、前进、后退、停止，实现行驶时的局部路径规划，保证小车无碰撞清洗；
16.步骤十：根据步骤八获取的清洗目标三维检测模型实时获取清洗对象(挡板、栅栏)的三维信息(位置、大小、距离)，匹配步骤三所得清洗对象3d模型，获取挡板、栅栏的准确清洗范围，通过plc控制摆动电机和往复电机实现对清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
17.所述步骤一中采集方法具体为：将固态面阵激光雷达固定在畜牧清洗自动驾驶车上，车沿畜牧舍可通行路径行驶，采集畜牧舍内点云数据，并保存至数据bag中。
18.所述步骤二中使用体素化网格下采样处理步骤一获取的数据bag中的每一帧点云数据，对点云数据进行去噪滤波；
19.体素化网格下采样算法步骤为：根据点云数据创建一个最小三维体素栅格，然后计算出需要划分的小立方栅格的边长l，根据l的大小将三维体素栅格分解成m
×n×
1个小栅格，栅格划分完毕后将点云数据放到相应的小栅格中，同时删除那些不包含数据点的小栅格，在每个小栅格中，将离小栅格重心最近的数据点保留下来，删除其余的数据点，以此对点云数据进行去噪滤波，获取较为干净的畜牧舍点云数据；其计算步骤如下：
20.(1)小立方栅格的边长l的确定，小立方栅格的边长与邻近点个数k成正比，与点云的平均密度成反比，当点云的平均密度小时，表示在固定空间内的点云数量少，那么应将l取大些来提高k邻近搜索的范围，以保证有足够多点云进行k近邻的计算；当点云的平均密度大时，则表示在固定空间内的点云数量较多，应将l取小些，以保证在最适当的范围内搜索，降低搜索的时间；
21.小立方体栅格边长为如式1-1：
[0022][0023]
式中：α是比例因子，用来调节小立方体栅格的边长；s是比例系数；g是小栅格中点云数据的数目；
[0024]
单位小栅格中包含的点云数据个数如式1-2：
[0025]
n＝n/v
ꢀꢀ
(1-2)
[0026]
式中：n表示点云数据中点的总数；v表示三维体素栅格的体积；
[0027]
三维体素栅格的体积如式1-3：
[0028]
v＝l
x
lylzꢀꢀ
(1-3)
[0029]
式中：l
x
表示点云x轴方向最大范围；l
x
表示点云y轴方向最大范围；l
x
表示点云z轴方向最大范围，适当的将长方体向外扩大，增加距离λ对其修正，三维体素栅格三边的长度是：
[0030][0031]
将式(1-2)、(1-3)代入式(1-1)可得公式1-5：
[0032][0033]
(2)将点云数据划分到小立方体栅格中，根据小立方体栅格的边长l，将点云数据划分为m
×n×
l个小立方体栅格，其中，m＝ceil(l
x
/l)，n＝ ceil(ly/l)，l＝ceil(lz/l)，ceil(x)为取整函数，表示不小于的最小整数，对于任一点pi，其所属小立方体栅格号如式1-6：
[0034][0035]
在整个栅格编码中，点pi的栅格编码为如果转换为一维编码的话，点pi的栅格编码如式1-7所示：
[0036][0037]
根据式(1-6)或式(1-7)点云数据模型的每个数据点的栅格编码，且将编码放进哈希链表，建立点云数据间的空间拓扑关系后，确定出每个数据点pi的邻近点，数据点pi的邻近点是在它本身所在的小栅格和相邻小栅格中的数据点，而k 邻域的确定是计算这些数据点到点pi的距离，并将按距离的远近，取出离pi点最近的前k个数据点，存入到哈希链表中，作为数据点pi的k邻域点，k取值范围一般为8～20；
[0038]
(3)散乱点云法向量的估算，利用最小二乘平面法来估计数据点的法向量，假设物体的采样表面是足够光滑的，则采样得到的物体表面的数据点就可以用最小二乘平面法来实现曲面拟合，根据点云数据模型中的每个数据点pi及其k近邻点，计算出每个数据点pi及其k近邻点的最小二乘局部平面h，公式如1-8所示：
[0039][0040]
式中：θ表示高斯权重；n表示最小二乘局部平面h的法向量，满足n2＝1；d表示最小二乘局部平面h与坐标原点两者之间的距离；
[0041]
这样三维点云数据模型的法向量就可以用最小二乘局部平面的法向量近似表示；最小二乘局部平面平面h的法向量可以通过主成分分析得到，数据点pi经过它的k邻域，如式1-9所示：
[0042][0043]
对式(1-9)分解，得到协方差矩阵m的3特征值λ0，λ1，λ2，设 0≤λ0≤λ1≤λ2，对应的3个特征向量分别是v0，v1，v2，则最小特征向量v0可以近似表示数据点pi的法向量，协方
差矩阵m如式1-10所示：
[0044][0045]
(4)代表点的选取及下采样实现，根据求出的点云数据模型中每个点pi的邻域和点云数据模型中每个数据点pi的法向量，求出每个数据点pi的法向量与其邻近点法向之间的夹角，若法向量之间的夹角越大，则说明数据点pi在其k 近邻附近的曲率变化比较大，存在尖锐的几何特征，相反，若法向量之间的夹角越小，则说明数据点pi在其k近邻附近的曲率变化比较小，存在稀疏的几何特征，因此，可以预先设定一个阈值，当数据点pi的法向量与其邻近点法向量之间的夹角大于阈值，则将该数据点视为特征点，调整边长比例因子α，使得立方体栅格边长变小，每个栅格点云数量变少，这样可以保留较多特征数据点，夹角小于阈值的点作为非特征点，调整边长比例因子α，使得立方体栅格变大，每个栅格点云数量变多，保留较少非特征数据点，每个小立方体栅格中，三维栅格中心集重心如式1-11所示：
[0046][0047]
式中g为小立方体栅格中点云数据个数，选取最靠近重心数据点保留下来，代表这个小立方体栅格所有数据点，实现点云下采样，完成对步骤一采集的点云数据进行滤波去噪，获取较为干净的畜牧舍点云数据。
[0048]
所述步骤三中使用ndt和icp融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3d模型；
[0049]
首先根据步骤二所得点云数据得到网格参数大小，配准距离误差数据，其次进行ndt算法粗配准，，并对配准后的点云进行30％的抽稀，具体算法为：
[0050]
(1)将点云数据划分为数个大小均匀的三维立方体，每个立方体中都包含有一定数量的点；
[0051]
(2)点云在立方体中服从高斯分布，因此点云数据的高斯分布参数，如式1-12，1-13：
[0052][0053]
[0054]
式中u表示均值向量，c表示协方差矩阵，n为立方体中总点数，xi为立方体中的一点，t表示转换矩阵；
[0055]
(3)当一点落入小方格内部时，就根据内部的u和c对该点进行打分s，如式1-14：
[0056][0057]
(4)最小化(3)中的目标函数即可得到转换矩阵t，如式1-15：
[0058][0059]
(5)利用求得的变换矩阵对获取点云进行旋转变换，融合到相邻的点云数据中，并对配准后的点云进行30％的抽稀，得到一个粗配准点云结果；
[0060]
然后进行kd树索引构建，搜索ndt粗配准点云的最近点，为icp精细配准提供配准最近点，具体算法为：
[0061]
(1)确定分割阈值：对所有的数据分别计算各个维度上数据的方差，选方差最大值作为所在维的分割域值；
[0062]
(2)确定根节点：按照分割域值对三维数据进行排序，选中间点作为kd 树的根节点；
[0063]
(3)确定左右子树空间：确定根节点后，使得所有小于根节点的点出现在左子树，剩余的点出现在右子树；
[0064]
(4)分别对左右子树的点重复(2)和(3)，直至每个子空间都只有一个点，kd树是一种递归算法，当搜索相邻点时，首先确定搜索路径上最近点，然后回溯，检查有无最近点，将查询点和最近点的距离与最近点和分割平面的距离进行对比，前者大则进入该点的子空间，否则继续回溯直至前者大于后者，最终确定最近点；
[0065]
最后通过icp算法进行精细配准，得到畜牧舍清洗对象的3d模型，具体算法为：
[0066]
(1)在点云b中取一点bi∈b；
[0067]
(2)在点云a中找出与之对应的点ai∈a使得ai-bi＝min；
[0068]
(3)计算出旋转矩阵r和平移矩阵t，使得误差函数最小；
[0069]
(4)使用上步得出的旋转矩阵r和平移矩阵t对bi进行变换，得到新的点集，如式1-16：
[0070]b′i＝rbi t，bi∈b
ꢀꢀ
(1-16)
[0071]
(5)计算b
′i与点集a中与之对应的点ai的平均距离，如式 1-17：
[0072]b′i＝rbi t，bi∈b
ꢀꢀ
(1-17)
[0073]
(6)若d小于设定的阈值或大于设定的迭代次数，则停止计算，否则返回步骤(2)，直到满足收敛条件为止。
[0074]
所述步骤四中使用改进的cartographer算法通过步骤二获得的处理过的点云数据进行配准建模，获取畜牧舍3d模型；
[0075]
首先采用前端local累加式建图，其主要步骤为：
[0076]
(1)对一帧点云的每个点数据，筛出小于指定距离的点，并逐个记录为 returns；对于大于指定距离的点，将该方向上一定距离的某点记入misses点集。此处可将多帧数据
一并存入并在后续工作中作一帧处理，默认仅累计一帧的数据；
[0077]
(2)对输入的点云数据经过修剪筛去了z轴阈值之外的点，之后分别对 returns点集和misses点集进行体素滤波，每个体素内仅保留一个点；
[0078]
(3)从姿态推算器中获取当前点云帧的姿态初值，并优化点云帧的位姿，匹配完成后把点云帧插入子图；
[0079]
(4)将当前帧与较活跃的子图进行匹配以优化当前帧位姿，首先通过real
‑ꢀ
timecsm(实时相关扫描匹配)对激光帧数据在submap的栅格地图上进行一次粗匹配，产生一个大体较好的位姿值并返回一个得分数；
[0080]
然后采用后端global位姿图构建与回环匹配，具体步骤为：
[0081]
(1)根据前端建图的local轨迹构建器和用于回环全局优化的位姿图，将每一帧的激光帧加入一个个子图，以完成实时累加式地图的建立，之后进行位姿图的构建，将匹配好的激光帧作为节点加入位姿图中进行后续全局匹配；
[0082]
(2)通过稀疏位姿调整方法来优化所有激光雷达数据帧和子图的位姿，激光雷达数据帧在插入子图时的位姿会被缓存到内存中进行闭环检测，当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测，cartographer算法使用稀疏位姿调整方法构建的优化问题的数学表达式为：
[0083][0084]
上式中，分别表示在一定约束条件下，子图的位姿和扫描帧的位姿。相对位姿表示扫描帧在子图中的匹配位置，与其相关的协方差矩阵共同构成优化约束。这个约束的代价函数使用残差表示：
[0085][0086][0087]
最后匹配出总的位姿图，即畜牧舍3d模型。
[0088]
所述步骤五先根据畜牧舍3d模型确定畜牧舍入口和出口坐标，采用a*算法规划出清洗畜牧舍的可通行路径；
[0089]
首先将畜牧舍环境m规定由栅格m
ij
构成：
[0090][0091]
其中，m
ij
＝0表示畜牧舍清洗驾驶车的起始位置单元栅格；m
ij
＝1表示无障碍区域单元栅格；m
ij
＝2表示障碍区域单元栅格；m
ij
＝3表示目标所处位置；
[0092]
栅格大小的选取对于整个路径规划是很重要的，栅格选取较大，计算量就会减少，但得到的路径长度可能会增大；相反栅格选取太小，路径规划的准确度就会提高，但规划过程缓慢，栅格大小主要由畜牧舍环境确定。栅格长度：
[0093][0094]
其中，r为障碍物半径，r为畜牧舍清洗驾驶车半径，δ为设定的安全距离。综上可以
将畜牧舍的路径规划问题概述为通过在已得到的环境模型中的无障碍区域搜索，得到一条由栅格组成的连续的最短路径；
[0095]
其次a
*
算法根据所定义的估价函数大小来确定最优路径，代价函数表示为：
[0096]
f(n)＝g(n) h(n)
ꢀꢀ
(1
‑ꢀ
24)
[0097]
其中，n为当前节点；f(n)为节点n的估价函数；g(n)为从起始点到当前节点n的实际代价值；h(n)为当前点n到目标点的估计代价值。一般来说h(n)为欧氏距离，其定义如下：
[0098][0099]
其中，(xn，yn)表示当前位置处栅格的中心坐标；(x
goal
，y
goal
)则表示目标栅格的中心。通过优化可以得到当前位置到畜牧舍出口位置的最短路径。
[0100]
所述步骤六中采用张正友算法对cmos摄像头进行标定，获取摄像头参数矩阵；
[0101]
张正友摄像头标定算法中像素坐标和世界坐标关系式如下所示。其中[r|t] 是外参矩阵，r是旋转矩阵，t是平移向量。左乘外参矩阵即可将世界坐标从世界坐标系转换到摄像机坐标系；
[0102]
定的具体步骤为：
[0103]
(1)制作黑白棋盘格，并固定在木板上；
[0104]
(2)固定摄像头，拍摄25组不同角度位置的棋盘格板图像；
[0105]
(3)将棋盘格图像导入matlab的cameracalibrator工具中，并选定棋盘格板边长为49mm；
[0106]
(4)工具自动标定棋盘格，获取摄像头各项参数。摄像头各项参数“radiadistortion”即为摄像头的畸变矩阵，“intrinsicmatrix”为参数矩阵；
[0107]
(5)根据摄像头参数矩阵和畸变参数矫正拍摄图像，以检测摄像头参数的准确性。
[0108]
所述步骤七利用solvepnp算法通过步骤六得到的参数矩阵，计算cmos摄像头到激光雷达的旋转矩阵和平移矩阵，获取到三维图像；
[0109]
solvepnp算法是利用多个控制点(三维空间坐标以及对应图像平面坐标) 在三维场景中的坐标及其在图像中的透视投影坐标即可求解出摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵r；
[0110]
根据该算法先摄像头与雷达固定在畜牧清洗自动驾驶车上，并在车前2米处固定一块白板，用cmos摄像头采集白板图像，且激光雷达采集相应点云数据，然后利用opencv获取鼠标左键点击位置图像上像素坐标(白板四角处)，同时通过cloudcompare点云工具获取点云数据相应位置的点云三维坐标。利用solvepnp算法通过所求的摄像头内参矩阵、畸变参数、图像二维坐标和点云三维坐标计算cmos摄像头到激光雷达的旋转矩阵和平移矩阵。最后利用旋转矩阵和平移矩阵将点云数据投影到二维图像上，融合摄像头图像和激光雷达
点云数据，获取到三维图像。
[0111]
所述步骤八先标注三维图像，采用pointrcnn算法对标定的数据集进行训练，得到专用于检测畜牧舍目标的三维检测模型，行驶时实时框取畜牧舍目标的三维检测框；
[0112]
首先需要对步骤六所得的三维图像进行三维标注，框选出图像中各类目标的三维检测框，同时也框选出相应点云数据里清洗对象的三维检测框，并采用 pointrcnn算法对标好的数据集进行训练，得到专用于畜牧舍目标的三维检测模型，其中pointrcnn算法训练检测模型分为两个阶段：
[0113]
第一阶段，对点云进行语义分割，对每个点得到一个预测label，比如对所有判断是“车”的点(也叫做前景点)，赋予label＝1，其他点(也叫做背景点)，赋予label＝0。然后，用所有前景点生成boundingbox，一个前景点对应一个boundingbox，然后用了去除冗余的方法，继续减少boundingbox的数目，这一阶段结束的时候只留下300个boundingbox；
[0114]
第二阶段，继续优化上一阶段生成的boundingbox。首先，对前一阶段生成的boundingbox做旋转平移，把这些boundingbox转换到自己的正规划坐标系下 (canonicalcoordinates)。然后，通过点云池化等操作的到每个boundingbox的特征，再结合第一阶段的到的特征，进行boundingbox的修正和置信度的打分，从而的到最终的boundingbox。由此得到畜牧舍内清洗对象的三维检测模型。
[0115]
所述步骤九中通过步骤八获取的畜牧舍目标三维检测模型实时检测障碍物，并获取其三维信息(位置、大小、距离)，根据获取的三维信息采用人工势场法规划出可通行局部路径，控制小车的左转、右转、前进、后退、停止，实现行驶时的局部路径规划，保证小车无碰撞清洗；
[0116]
首先对控制器发送命令的串口进行测试，串口可正常发送数据时，根据步骤七获取的清洗对象检测模型实时检测挡板、工作人员等障碍物，并获取其位置、大小、距离三维信息，再根据获取的三维信息采用人工势场法规划出可通行局部路径，具体做法为：
[0117]
(1)将畜牧清洗自动驾驶车、畜牧舍出口与障碍三者间的相对速度关系作为优化路径的指标。引力势场函数u
att
可定义为：
[0118][0119]
相应的引力f
att
可表示：
[0120]fatt
(q)＝mρ(q，qg) k
v1
ρ(v，vg)
ꢀꢀ
(1
‑ꢀ
28)
[0121]
式中，m、k
v1
为引力场正常量；ρ(q，qg)为畜牧清洗自动驾驶车与畜牧舍出口之间的欧氏距离；ρ(v，vg)为畜牧清洗自动驾驶车和畜牧舍出口之间的相对速度；
[0122]
斥力势场函数u
rep
可定义为：
[0123][0124]urepv
＝k
v2
ρ2(v，v
obs
)/2
[0125]
(1-30)
[0126][0127]
相应斥力f
rep
可表示为：
[0128][0129]frepv
＝k
v2
ρ(v，v
obs
)
[0130]
(1-33)
[0131][0132]
其中，k、k
v2
为斥力场正常量；ρ(q，q
obs
)表示畜牧清洗自动驾驶车与障碍物两点之间的欧氏距离；ρ(v，v
obs
)表示畜牧清洗自动驾驶车和动态障碍物之间的相对速度；ρ0为常数，表示障碍物对畜牧清洗自动驾驶车产生作用的最大范围；α为障碍物的相对移动方向和移动畜牧清洗自动驾驶车与障碍物连线的夹角，当时，障碍物的移动方向确定为靠近移动畜牧清洗自动驾驶车；
[0133]
(2)当障碍物和畜牧舍出口对移动畜牧清洗自动驾驶车产生的排斥力和吸引力相等时，增设虚拟子目标法，借助虚拟外力，使畜牧清洗自动驾驶车摆脱陷阱，成功抵达出口。首先依据畜牧清洗自动驾驶车的状态信息判断是否进入局部最小值陷阱，以连续五个步长作为判断依据，当总的移动距离小于βl(l 为步长，β∈[2，4]时，改变β，提前检测到自身陷入局部极小值陷阱。其次程序跳转到虚拟子目标增设模块，存储使移动畜牧清洗自动驾驶车陷入局部最小值陷阱的障碍物位置信息，形成障碍物群，充分考虑出口位置与障碍物群位置信息，判断障碍物在移动畜牧清洗自动驾驶车与出口连接线左右两端数目，选择障碍物群左侧或右侧为目标障碍物，然后利用下式得到虚拟子目标位置：
[0134]
式中，x
ob
，y
ob
为目标障碍物位置；(xg，yg)为原畜牧舍出口所处位置； (x，y)为畜牧清洗自动驾驶车的当前位置；β1，β2为可调参数，β1，β2＞0；移动畜牧清洗自动驾驶车摆脱上述陷阱后，将撤出虚拟子目标，移动畜牧清洗自动驾驶车不再受外力作用。移动畜牧清洗自动驾驶车将在原目标和障碍物的合力作用下向原畜牧舍出口靠近；
[0135]
(3)当畜牧清洗自动驾驶车探测到的障碍物数目大于设定值或畜牧清洗自动驾驶车探测到障碍物与自身相对位置小于设定值时，环境即为复杂环境，在这种情况下，畜牧清洗自动驾驶车步长取l
min
；而当畜牧清洗自动驾驶车离目标很近时，为避免出现步长太大而越过过出口，或者步长太小而无法跟踪出口的情况，步长取l
mid
；当畜牧清洗自动驾驶车处于简单环境时，步长取 l
max
。规划出一条局部路径；
[0136]
随后上位机通过串口将路径信息发送给主控制器，主控器将上位机发来的路径信息通过串口中断进行接收，打包完成后然后通过can总通信将畜牧清洗自动驾驶车的前进、
后退、刹车、左转和右转对应的控制命令发送到总线上，然后设置一个can总线发送标志位，当一个模块的数据打包完成后将标志位flag置1，然后通过can总线发送该模块的动作信息给从控制器；
[0137]
从控制器通过can总线方式与主控器相连。从控制器包括两部分设计，一是can总线接收软件设计，二是使用pwm方式对各个舵机的脉冲宽度进行控制，实现畜牧清洗车的前进、后退、刹车和转向；
[0138]
从控制器can总线接收设计采用stm32f103zet6自带的一个bx_can 控制器、收发器是tja1050芯片。从控制器上电后需要进行can总线部分的初始化，其中主要包括对从节点号、过滤器、波特率、中断的设置。从节点从 can总线上接收数据，然后can控制器对所接收数据进行过滤，得到对应各控制的数据信息；
[0139]
从控制器pwm舵机舵角控制设计采用的是stm32f103zet6，它具有两个 16位高级定时器tim1、tim8(带死区控制，有互补输出)和四个16位通用定时器tim2～tim5，均可输出pwm，产生dma请求，以此完成畜牧清洗车的前进、后退、刹车和转向。
[0140]
所述步骤十根据步骤八获取的畜牧舍目标三维检测模型实时获取清洗对象 (挡板、栅栏)的三维信息，匹配步骤三所得畜牧舍清洗对象3d模型，获取挡板、栅栏的准确清洗范围，通过plc控制摆动电机和往复电机实现对清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗；
[0141]
首先测试plc串口是否连接成功，其次根据实时获取的挡板、栅栏的位置、大小、距离三维信息，匹配步骤三获取的畜牧舍清洗对象3d场景模型，以确定清洗目标的范围，若检测到栅栏时，通过plc控制摆动电机控制清洗喷头纵向摆动，保证栅栏全方位清洗，若检测到挡板时，确定栅栏位置及大小形状信息，以锁定死角位置，随后通过plc控制往复电机控制清洗杆上下摆动，达到死角反复清洗的目的，从而全方位清洗畜牧舍。
[0142]
本发明的有益效果：
[0143]
本发明一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法主要分为两大部分，自动驾驶系统和畜牧清洗系统。自动驾驶系统的信号采集是由超声波测距传感器、柔性薄膜压力传感器、九轴姿态传感器组成，通过采集到的信息结合激光雷达的采集的深度信息和机器视觉采集的图像信息，由主控制器控制两个舵机模块实现自动驾驶功能；畜牧清洗系统是根据激光雷达和机器视觉检测出清洗目标(栅栏、挡板)的三维目标信息，匹配该三维目标信息，控制器控制车速及清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
[0144]
该发明可适用多种畜牧场景，普适性高，且融入自动驾驶技术达到高效率、成本低、省人力的效果，因此畜牧舍清洗自动驾驶车将会成为一个新的技术研究热点，带动畜牧业加快发展。
[0145]
该发明可达到以下几点。
[0146]
(1)畜牧舍可通行道路的自动规划及行驶，行驶无碰撞，规划路径保证可清洗到所有畜牧舍；
[0147]
(2)畜牧舍围栏和死角清洗，清洗面积大于等于97％，地面残留有机物小于等于5％；
[0148]
(3)雷达对畜牧舍场景实时建模；
[0149]
(4)采用摄像机和激光雷达对畜牧舍内三维目标检测，实现畜牧舍内栅栏、挡板、工作人员等实时三维检测，实时检测准确率大于等于95％；
[0150]
(5)建模、目标检测可实时显示；
[0151]
(6)可实现自动驾驶和手动驾驶切换，自动手动切换时长小于等于3s。
附图说明
[0152]
图1是本发明流程示意图。
[0153]
图2是本发明实例提供的点云配准流程图。
[0154]
图3是本发明实例提供的基于机器视觉的多目标识别算法设计流程图。
具体实施方式
[0155]
下面结合附图对本发明作进一步详细说明。
[0156]
如图1-图3所示：首先通过激光雷达采用改进的cartographer算法对整个畜牧舍进行建模，得到3d畜牧舍模型，并根据模型采用a*和人工势场相结合的算法规划出清洗畜牧舍的可通行路径。其次采用solvepnp算法融合激光雷达采集的点云数据和摄像头采集的图像，并通过pointrcnn算法训练出专用于畜牧舍清洗对象的检测模型，畜牧清洗自动驾驶车沿规划的路径行驶时，根据激光雷达和机器视觉检测出清洗目标(栅栏、挡板)的三维目标信息，识别检测目标，并框取三维检测框。根据获取到的目标三维检测框匹配三维目标信息，利用主控制器控制两个舵机模块进行局部路径规划，同时依据清洗目标和清洗范围，然后从控制器控制车速及plc控制清洗喷头的纵向摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
[0157]
下面结合附图对本发明的应用原理作进一步的说明：
[0158]
如图1所示为本发明方法的总体算法流程图，本发明所述的融合激光雷达和机器视觉的畜牧清洗自动驾驶车，按以下步骤进行：
[0159]
步骤一：将固态面阵激光雷达固定在畜牧清洗自动驾驶车上，车沿畜牧舍可通行路径行驶，采集畜牧舍内点云数据，并保存至数据bag中。
[0160]
步骤二：使用体素化网格下采样处理步骤一获取的数据bag中的每一帧点云数据，对点云数据进行去噪滤波；
[0161]
体素化网格下采样算法步骤为：根据点云数据创建一个最小三维体素栅格，然后计算出需要划分的小立方栅格的边长l，根据l的大小将三维体素栅格分解成m
×n×
1个小栅格，栅格划分完毕后将点云数据放到相应的小栅格中，同时删除那些不包含数据点的小栅格，在每个小栅格中，将离小栅格重心最近的数据点保留下来，删除其余的数据点，以此对点云数据进行去噪滤波，获取较为干净的畜牧舍点云数据。其计算步骤如下：
[0162]
(1)小立方栅格的边长l的确定。在体素化栅格下采样点云简化算法中，小立方体栅格边长l的选取十分重要：过大的栅格会降低搜索效率，过小的栅格则会出现空的栅格。小立方栅格的边长与邻近点个数k成正比，与点云的平均密度成反比，当点云的平均密度小时，表示在固定空间内的点云数量少，那么应将l取大些来提高k邻近搜索的范围，以保证有足够多点云进行k近邻的计算；当点云的平均密度大时，则表示在固定空间内的点云数量较多，应将l 取小些，以保证在最适当的范围内搜索，降低搜索的时间；
[0163]
小立方体栅格边长为如式1-1：
[0164][0165]
式中：α是比例因子，用来调节小立方体栅格的边长；s是比例系数；g是小栅格中点云数据的数目；
[0166]
单位小栅格中包含的点云数据个数如式1-2：
[0167]
n＝n/v
ꢀꢀ
(1-2)
[0168]
式中：n表示点云数据中点的总数；v表示三维体素栅格的体积；
[0169]
三维体素栅格的体积如式1-3：
[0170]
v＝l
x
lylzꢀꢀ
(1-3)
[0171]
式中：l
x
表示点云x轴方向最大范围；l
x
表示点云y轴方向最大范围；l
x
表示点云z轴方向最大范围。而为了方便后续的统计，应确保数据点不会出现在外接三维体素栅格的角、边和面上。因此，适当的将长方体向外扩大，增加距离λ对其修正。所以，三维体素栅格三边的长度应该是：
[0172][0173]
将式(1-2)、(1-3)代入式(1-1)可得公式1-5：
[0174][0175]
(2)将点云数据划分到小立方体栅格中。根据小立方体栅格的边长l，将点云数据划分为m
×n×
l个小立方体栅格，其中，m＝ceil(l
x
/l)，n＝ceil(ly/l)，l＝ceil(lz/l)。ceil(x)为取整函数，表示不小于的最小整数。对于任一点pi，其所属小立方体栅格号如式1-6：
[0176][0177]
在整个栅格编码中，点pi的栅格编码为如果转换为一维编码的话，点pi的栅格编码如式1-7所示：
[0178][0179]
根据式(1-6)或式(1-7)点云数据模型的每个数据点的栅格编码，且将编码放进哈希链表，建立点云数据间的空间拓扑关系后，确定出每个数据点pi的邻近点。数据点pi的邻近点是在它本身所在的小栅格和相邻小栅格中的数据点。而k 邻域的确定是计算这些数据点到点pi的距离，并将按距离的远近，取出离pi点最近的前k个数据点，存入到哈希链表中，作为数据点pi的k邻域点。k的取值，要综合考虑物体表面的凹凸性和点云数据模型的密度，并且要确保重建得到的曲面在每个数据点的邻近范围内是单凸性或者单凹性的，故k取值范围一般为8～20；
[0180]
(3)散乱点云法向量的估算。利用最小二乘平面法来估计数据点的法向量，假设物体的采样表面是足够光滑的，则采样得到的物体表面的数据点就可以用最小二乘平面法来实现曲面拟合。根据点云数据模型中的每个数据点pi及其k近邻点，计算出每个数据点pi及其k近邻点的最小二乘局部平面h，公式如1-8所示：
[0181][0182]
式中：θ表示高斯权重；n表示最小二乘局部平面h的法向量，满足n2＝1； d表示最小二乘局部平面h与坐标原点两者之间的距离；
[0183]
这样三维点云数据模型的法向量就可以用最小二乘局部平面的法向量近似表示。最小二乘局部平面平面h的法向量可以通过主成分分析得到，数据点pi经过它的k邻域，如式1-9所示：
[0184][0185]
对式(1-9)分解，得到协方差矩阵m的3特征值λ0，λ1，λ2，设 0≤λ0≤λ1≤λ2，对应的3个特征向量分别是v0，v1，v2，则最小特征向量v0可以近似表示数据点pi的法向量，协方差矩阵m如式1-10所示：
[0186][0187]
(4)代表点的选取及下采样实现。根据求出的点云数据模型中每个点pi的邻域和点云数据模型中每个数据点pi的法向量，求出每个数据点pi的法向量与其邻近点法向之间的夹角。若法向量之间的夹角越大，则说明数据点pi在其k 近邻附近的曲率变化比较大，存在尖锐的几何特征。相反，若法向量之间的夹角越小，则说明数据点pi在其k近邻附近的曲率变化比较小，存在稀疏的几何特征。因此，可以预先设定一个阈值，当数据点pi的法向量与其邻近点法向量之间的夹角大于阈值，则将该数据点视为特征点，调整边长比例因子α，使得立方体栅格边长变小，每个栅格点云数量变少，这样可以保留较多特征数据点。夹角小于阈值的点作为非特征点，调整边长比例因子α，使得立方体栅格变大，每个栅格点云数量变多，保留较少非特征数据点。每个小立方体栅格中，三维栅格中心集重心如式1-11所示：
[0188]
[0189]
式中g为小立方体栅格中点云数据个数。选取最靠近重心数据点保留下来，代表这个小立方体栅格所有数据点，实现点云下采样。完成对步骤一采集的点云数据进行滤波去噪，获取较为干净的畜牧舍点云数据。
[0190]
步骤三：使用ndt和icp融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3d模型；
[0191]
首先根据步骤二所得点云数据得到网格参数大小，配准距离误差数据，其次进行ndt算法粗配准，，并对配准后的点云进行30％的抽稀，具体算法为：
[0192]
(1)将点云数据划分为数个大小均匀的三维立方体，每个立方体中都包含有一定数量的点；
[0193]
(2)点云在立方体中服从高斯分布，因此点云数据的高斯分布参数，如式 1-12，1-13：
[0194][0195][0196]
式中u表示均值向量，c表示协方差矩阵，n为立方体中总点数，xi为立方体中的一点，t表示转换矩阵；
[0197]
(3)当一点落入小方格内部时，就根据内部的u和c对该点进行打分s，如式1-14：
[0198][0199]
(4)最小化(3)中的目标函数即可得到转换矩阵t，如式1-15：
[0200][0201]
(5)利用求得的变换矩阵对获取点云进行旋转变换，融合到相邻的点云数据中，并对配准后的点云进行30％的抽稀，得到一个粗配准点云结果。
[0202]
然后进行kd树索引构建，搜索ndt粗配准点云的最近点，为icp精细配准提供配准最近点，具体算法为：
[0203]
(1)确定分割阈值：对所有的数据分别计算各个维度上数据的方差，选方差最大值作为所在维的分割域值；
[0204]
(2)确定根节点：按照分割域值对三维数据进行排序，选中间点作为kd 树的根节点；
[0205]
(3)确定左右子树空间：确定根节点后，使得所有小于根节点的点出现在左子树，剩余的点出现在右子树；
[0206]
(4)分别对左右子树的点重复(2)和(3)，直至每个子空间都只有一个点。kd树是一种递归算法，当搜索相邻点时，首先确定搜索路径上最近点，然后回溯，检查有无最近点，将查询点和最近点的距离与最近点和分割平面的距离进行对比，前者大则进入该点的子空间，否则继续回溯直至前者大于后者，最终确定最近点。
[0207]
最后通过icp算法进行精细配准，得到畜牧舍清洗对象的3d模型，具体算法为：
[0208]
(1)在点云b中取一点bi∈b；
[0209]
(2)在点云a中找出与之对应的点ai∈a使得ai-bi＝min；
[0210]
(3)计算出旋转矩阵r和平移矩阵t，使得误差函数最小；
[0211]
(4)使用上步得出的旋转矩阵r和平移矩阵t对bi进行变换，得到新的点集，如式1-16：
[0212]b′i＝rbi t，bi∈b
ꢀꢀ
(1-16)
[0213]
(5)计算b
′i与点集a中与之对应的点ai的平均距离，如式 1-17：
[0214]b′i＝rbi t，bi∈b
ꢀꢀ
(1-17)
[0215]
(6)若d小于设定的阈值或大于设定的迭代次数，则停止计算，否则返回步骤(2)，直到满足收敛条件为止。
[0216]
步骤四：使用改进的cartographer算法通过步骤二获得的处理过的点云数据进行配准建模，获取畜牧舍3d模型；
[0217]
首先采用前端local累加式建图，其主要步骤为：
[0218]
(1)对一帧点云的每个点数据，筛出小于指定距离的点，并逐个记录为 returns；对于大于指定距离的点，将该方向上一定距离的某点记入misses点集。此处可将多帧数据一并存入并在后续工作中作一帧处理，默认仅累计一帧的数据；
[0219]
(2)对输入的点云数据经过修剪筛去了z轴阈值之外的点。之后分别对 returns点集和misses点集进行体素滤波。每个体素内仅保留一个点；
[0220]
(3)从姿态推算器中获取当前点云帧的姿态初值，并优化点云帧的位姿。匹配完成后把点云帧插入子图；
[0221]
(4)将当前帧与较活跃的子图进行匹配以优化当前帧位姿，首先通过real
‑ꢀ
timecsm(实时相关扫描匹配)对激光帧数据在submap的栅格地图上进行一次粗匹配，产生一个大体较好的位姿值并返回一个得分数。
[0222]
然后采用后端global位姿图构建与回环匹配，具体步骤为：
[0223]
(1)根据前端建图的local轨迹构建器和用于回环全局优化的位姿图，将每一帧的激光帧加入一个个子图，以完成实时累加式地图的建立，之后进行位姿图的构建，将匹配好的激光帧作为节点加入位姿图中进行后续全局匹配；
[0224]
(2)通过稀疏位姿调整方法来优化所有激光雷达数据帧和子图的位姿，激光雷达数据帧在插入子图时的位姿会被缓存到内存中进行闭环检测。当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测。cartographer算法使用稀疏位姿调整方法构建的优化问题的数学表达式为：
[0225][0226]
上式中，分别表示在一定约束条件下，子图的位姿和扫描帧的位姿。相对位姿表示扫描帧在子图中的匹配位置，与其相关的协方差矩阵共同构成优化约束。这个约束的代价函数使用残差表示：
[0227]
[0228][0229]
最后匹配出总的位姿图，即畜牧舍3d模型。
[0230]
步骤五：根据畜牧舍3d模型，确定畜牧舍入口和出口坐标，采用a*算法规划出清洗畜牧舍挡板、围栏的可通行路径；
[0231]
首先将畜牧舍环境m规定由栅格m
ij
构成：
[0232]
m＝{m
ij
，m
ij
＝0，1，2，3}
ꢀꢀ
(1
‑ꢀ
22)
[0233]
其中，m
ij
＝0表示畜牧舍清洗驾驶车的起始位置单元栅格；m
ij
＝1表示无障碍区域单元栅格；m
ij
＝2表示障碍区域单元栅格；m
ij
＝3表示目标所处位置。
[0234]
栅格大小的选取对于整个路径规划是很重要的，栅格选取较大，计算量就会减少，但得到的路径长度可能会增大；相反栅格选取太小，路径规划的准确度就会提高，但规划过程缓慢。栅格大小主要由畜牧舍环境确定。栅格长度：
[0235][0236]
其中，r为障碍物半径，r为畜牧舍清洗驾驶车半径，δ为设定的安全距离。综上可以将畜牧舍的路径规划问题概述为通过在已得到的环境模型中的无障碍区域搜索，得到一条由栅格组成的连续的最短路径；
[0237]
其次a
*
算法根据所定义的估价函数大小来确定最优路径。代价函数表示为：
[0238]
f(n)＝g(n) h(n)
ꢀꢀ
(1
‑ꢀ
24)
[0239]
其中，n为当前节点；f(n)为节点n的估价函数；g(n)为从起始点到当前节点n的实际代价值；h(n)为当前点n到目标点的估计代价值。一般来说h(n)为欧氏距离，其定义如下：
[0240][0241]
其中，(xn，yn)表示当前位置处栅格的中心坐标；(x
goal
，y
goal
)则表示目标栅格的中心。通过优化可以得到当前位置到畜牧舍出口位置的最短路径。
[0242]
步骤六：采用张正友算法对cmos摄像头进行标定，获取摄像头参数矩阵；
[0243]
张正友摄像头标定算法中像素坐标和世界坐标关系式如下所示。其中[r|t] 是外参矩阵，r是旋转矩阵，t是平移向量。左乘外参矩阵即可将世界坐标从世界坐标系转换到摄像机坐标系。
[0244][0245]
标定的具体步骤为：
[0246]
(1)制作黑白棋盘格，并固定在木板上；
[0247]
(2)固定摄像头，拍摄25组不同角度位置的棋盘格板图像；
[0248]
(3)将棋盘格图像导入matlab的cameracalibrator工具中，并选定棋盘格板边长
为49mm；
[0249]
(4)工具自动标定棋盘格，获取摄像头各项参数。摄像头各项参数中“radiadistortion”即为摄像头的畸变矩阵，“intrinsicmatrix”为参数矩阵；
[0250]
(5)根据摄像头参数矩阵和畸变参数矫正拍摄图像，以检测摄像头参数的准确性。
[0251]
步骤七：利用solvepnp算法通过步骤六得到的参数矩阵，计算cmos摄像头到激光雷达的旋转矩阵和平移矩阵，获取到三维图像；
[0252]
solvepnp算法是利用多个控制点(三维空间坐标以及对应图像平面坐标) 在三维场景中的坐标及其在图像中的透视投影坐标即可求解出摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵r；
[0253]
根据该算法先摄像头与雷达固定在畜牧清洗自动驾驶车上，并在车前2米处固定一块白板，用cmos摄像头采集白板图像，且激光雷达采集相应点云数据，然后利用opencv获取鼠标左键点击位置图像上像素坐标(白板四角处)，同时通过cloudcompare点云工具获取点云数据相应位置的点云三维坐标。利用solvepnp算法通过所求的摄像头内参矩阵、畸变参数、图像二维坐标和点云三维坐标计算cmos摄像头到激光雷达的旋转矩阵和平移矩阵。最后利用旋转矩阵和平移矩阵将点云数据投影到二维图像上，融合摄像头图像和激光雷达点云数据，获取到三维图像。
[0254]
步骤八：标注三维图像，采用pointrcnn算法对标定的数据集进行训练，得到专用于检测畜牧舍目标的三维检测模型，行驶时实时框取畜牧舍目标的三维检测框；
[0255]
首先需要对步骤六所得的三维图像进行三维标注，框选出图像中各类目标的三维检测框，同时也框选出相应点云数据里清洗对象的三维检测框，并采用 pointrcnn算法对标好的数据集进行训练，得到专用于畜牧舍目标的三维检测模型，其中pointrcnn算法训练检测模型分为两个阶段：
[0256]
第一阶段，对点云进行语义分割，对每个点得到一个预测label，比如对所有判断是“车”的点(也叫做前景点)，赋予label＝1，其他点(也叫做背景点)，赋予label＝0。然后，用所有前景点生成boundingbox，一个前景点对应一个boundingbox，然后用了去除冗余的方法，继续减少boundingbox的数目，这一阶段结束的时候只留下300个boundingbox；
[0257]
第二阶段，继续优化上一阶段生成的boundingbox。首先，对前一阶段生成的boundingbox做旋转平移，把这些boundingbox转换到自己的正规划坐标系下 (canonicalcoordinates)。然后，通过点云池化等操作的到每个boundingbox的特征，再结合第一阶段的到的特征，进行boundingbox的修正和置信度的打分，从而的到最终的boundingbox。由此得到畜牧舍内清洗对象的三维检测模型。
[0258]
步骤九：通过步骤八获取的畜牧舍目标三维检测模型实时检测障碍物，并获取其三维信息(位置、大小、距离)，根据获取的三维信息控制小车的左转、右转、前进、后退、停止，实现行驶时的局部路径规划，保证小车无碰撞清洗；
[0259]
首先对控制器发送命令的串口进行测试，串口可正常发送数据时，根据步骤七获取的清洗对象检测模型实时检测挡板、工作人员等障碍物，并获取其位置、大小、距离三维信息，再根据获取的三维信息采用人工势场法规划出可通行局部路径，具体做法为：
[0260]
(1)将畜牧清洗自动驾驶车、畜牧舍出口与障碍三者间的相对速度关系作为优化路径的指标。引力势场函数u
att
可定义为：
[0261]uatt
(q)＝[mρ2(q，qg) k
v1
ρ2(v，vg)]/2
ꢀꢀ
(1
‑ꢀ
17)
[0262]
相应的引力f
att
可表示：
[0263]fatt
(q)＝mρ(q，qg) k
v1
ρ(x，vg)
[0264]
(1-28)
[0265]
式中，m、k
v1
为引力场正常量；ρ(q，qg)为畜牧清洗自动驾驶车与畜牧舍出口之间的欧氏距离；ρ(v，vg)为畜牧清洗自动驾驶车和畜牧舍出口之间的相对速度；
[0266]
斥力势场函数u
rep
可定义为：
[0267][0268]urepv
＝k
v2
ρ2(v，v
obs
)/2
[0269]
(1-30)
[0270][0271]
相应斥力f
rep
可表示为：
[0272][0273]frepv
＝k
v2
ρ(v，v
obs
)
[0274]
(1-33)
[0275][0276]
其中，k、k
v2
为斥力场正常量；ρ(q，q
obs
)表示畜牧清洗自动驾驶车与障碍物两点之间的欧氏距离；ρ(v，v
obs
)表示畜牧清洗自动驾驶车和动态障碍物之间的相对速度；ρ0为常数，表示障碍物对畜牧清洗自动驾驶车产生作用的最大范围；α为障碍物的相对移动方向和移动畜牧清洗自动驾驶车与障碍物连线的夹角，当时，障碍物的移动方向确定为靠近移动畜牧清洗自动驾驶车；
[0277]
(2)当障碍物和畜牧舍出口对移动畜牧清洗自动驾驶车产生的排斥力和吸引力相等时，增设虚拟子目标法，借助虚拟外力，使畜牧清洗自动驾驶车摆脱陷阱，成功抵达出口。首先依据畜牧清洗自动驾驶车的状态信息判断是否进入局部最小值陷阱，以连续五个步长作为判断依据，当总的移动距离小于βl(l 为步长，β∈[2，4]时，改变β，提前检测到自身陷入局部极小值陷阱。其次程序跳转到虚拟子目标增设模块，存储使移动畜牧清洗自动驾驶车陷入局部最小值陷阱的障碍物位置信息，形成障碍物群，充分考虑出口位置与障碍物群位置信息，判断障碍物在移动畜牧清洗自动驾驶车与出口连接线左右两端数目，选择障碍物群左侧或右侧为目标障碍物，然后利用下式得到虚拟子目标位置：
[0278][0279]
式中，x
ob
，y
ob
为目标障碍物位置；(xg，yg)为原畜牧舍出口所处位置； (x，y)为畜牧清洗自动驾驶车的当前位置；β1，β2为可调参数，β1，β2＞0；移动畜牧清洗自动驾驶车摆脱上述陷阱后，将撤出虚拟子目标，移动畜牧清洗自动驾驶车不再受外力作用。移动畜牧清洗自动驾驶车将在原目标和障碍物的合力作用下向原畜牧舍出口靠近；
[0280]
(3)当畜牧清洗自动驾驶车探测到的障碍物数目大于设定值或畜牧清洗自动驾驶车探测到障碍物与自身相对位置小于设定值时，环境即为复杂环境。在这种情况下，畜牧清洗自动驾驶车步长取l
min
；而当畜牧清洗自动驾驶车离目标很近时，为避免出现步长太大而越过过出口，或者步长太小而无法跟踪出口的情况，步长取l
mid
；当畜牧清洗自动驾驶车处于简单环境时，步长取l
max
。规划出一条局部路径。
[0281]
随后上位机通过串口将路径信息发送给主控制器，主控器将上位机发来的路径信息通过串口中断进行接收，打包完成后然后通过can总通信将畜牧清洗自动驾驶车的前进、后退、刹车、左转和右转对应的控制命令发送到总线上，然后设置一个can总线发送标志位，当一个模块的数据打包完成后将标志位flag置1，然后通过can总线发送该模块的动作信息给从控制器；
[0282]
从控制器通过can总线方式与主控器相连。从控制器包括两部分设计，一是can总线接收软件设计，二是使用pwm方式对各个舵机的脉冲宽度进行控制，实现畜牧清洗车的前进、后退、刹车和转向；
[0283]
从控制器can总线接收设计采用stm32f103zet6自带的一个bx_can 控制器、收发器是tja1050芯片。从控制器上电后需要进行can总线部分的初始化，其中主要包括对从节点号、过滤器、波特率、中断的设置。从节点从 can总线上接收数据，然后can控制器对所接收数据进行过滤，得到对应各控制的数据信息；
[0284]
从控制器pwm舵机舵角控制设计采用的是stm32f103zet6，它具有两个 16位高级定时器tim1、tim8(带死区控制，有互补输出)和四个16位通用定时器tim2～tim5，均可输出pwm，产生dma请求，以此完成畜牧清洗车的前进、后退、刹车和转向。
[0285]
步骤十：根据步骤八获取的畜牧舍目标三维检测模型实时获取清洗对象 (挡板、栅栏)的三维信息，匹配步骤三所得畜牧舍清洗对象3d模型，获取挡板、栅栏的准确清洗范围，通过plc控制摆动电机和往复电机实现对清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗；
[0286]
首先测试plc串口是否连接成功，其次根据实时获取的挡板、栅栏的位置、大小、距离三维信息，匹配步骤三获取的畜牧舍清洗对象3d场景模型，以确定清洗目标的范围，若检测到栅栏时，通过plc控制摆动电机控制清洗喷头纵向摆动，保证栅栏全方位清洗，若检测到挡板时，确定栅栏位置及大小形状信息，以锁定死角位置，随后通过plc控制往复电机控制清洗杆上下摆动，达到死角反复清洗的目的，从而全方位清洗畜牧舍。