CN114219910A - 一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法 - Google Patents

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

Info

Publication number
CN114219910A
CN114219910A CN202111075136.5A CN202111075136A CN114219910A CN 114219910 A CN114219910 A CN 114219910A CN 202111075136 A CN202111075136 A CN 202111075136A CN 114219910 A CN114219910 A CN 114219910A
Authority
CN
China
Prior art keywords
point
livestock
data
cleaning
point cloud
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111075136.5A
Other languages
English (en)
Inventor
李颀
李煜哲
叶小敏
郭梦媛
冯文斌
马凯
张冉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shaanxi University of Science and Technology
Original Assignee
Shaanxi University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shaanxi University of Science and Technology filed Critical Shaanxi University of Science and Technology
Priority to CN202111075136.5A priority Critical patent/CN114219910A/zh
Publication of CN114219910A publication Critical patent/CN114219910A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods
    • G06T5/70—
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G06T7/85—Stereo camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Abstract

一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，首先通过激光雷达构建3D畜牧舍模型，根据模型采用A*算法规划出可完整清洗畜牧舍的可通行路径。其次激光雷达和机器视觉框取目标三维检测框。通过三维检测框匹配对应的3D模型获取准确位置信息，依据障碍目标的信息，采用人工势场法进行局部路径规划。处理器通过CAN总线发送行驶指令至微控器，实现车的前进、后退、刹车、转向和调速功能，处理器通过清洗目标信息，确定清洗范围和方式。PLC控制清洗喷头摆动和清洗杆的旋转，实现自适应畜牧舍清洗对象尺寸，完成对畜牧舍的栅栏、挡板等全方位清洗。本发明可适用多种畜牧场景，普适性高，且融入自动驾驶技术达到高效率、成本低、省人力的效果。

Description

一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法
技术领域
本发明涉及计算机视觉技术、激光雷达建模领域，特别涉及一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法。
背景技术
随着科技的发展，人工智能技术已经开始渗入畜牧行业，然而国内畜牧业的智能化程度还处于低级阶段，猪舍、牛舍、羊舍的清洗大部分仍是由人工完成的，耗费人力，效率较低；畜牧智能设备普及率低、多依赖于进口且成本高；现有清洗设备普适率低，无法达全方位清洗。因此高效率、成本低、适应性广的畜牧清洗自动驾驶车开发刻不容缓。而自动驾驶加入畜牧业清洗将会成为一个新的技术研究热点，带动畜牧业加快发展。
目前市场上推出的畜牧自动清洗车大多成本高，且无法适用不同环境得畜牧舍，清洗不彻底。因此性价比与推广性达不到市场要求。
发明内容
为了克服上述现有技术的不足，本发明的目的在于提供一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，利用固态面阵激光雷达对整个畜牧舍进行建模，得到3D畜牧舍模型，根据模型规划可清洗整个畜牧舍路径，行驶时根据激光雷达和机器视觉检测出清洗目标(栅栏、挡板)的三维目标信息，匹配畜牧舍清洗目标三维模型信息，然后控制车速及清洗喷头的纵向摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
为了实现上述功能，本发明采用的技术方案是：
一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，包括以下步骤；
步骤一，使用激光雷达采集畜牧舍点云数据；
步骤二，使用体素化网格下采样的算法处理点云数据，对点云数据进行去噪滤波，获取较为干净的畜牧舍点云数据；
步骤三：使用NDT和ICP融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3D模型；
步骤四：使用Cartographer算法构建畜牧舍3D模型，利用rosbag录制激光雷达的3D数据和IMU的数据，采用Cartographer算法根据录制的数据构建模型，获取畜牧舍3D模型；
步骤五：根据步骤四所得畜牧舍3D模型，确定畜牧舍入口和出口坐标，采用A*算法规划出清洗畜牧舍挡板、围栏的可通行路径；
步骤六：制作棋盘标定板，采用张正友算法对CMOS摄像头进行标定，获取CMOS摄像头参数矩阵；
步骤七：使用车上固定的CMOS摄像头和激光雷达采集图像、点云，利用 solvePnP算法通过步骤六得到的参数矩阵，计算CMOS摄像头到激光雷达的旋转矩阵和平移矩阵，最后通过获取的旋转矩阵将点云数据投影到二维图像上，融合摄像头图像和激光雷达点云数据，获取到三维图像；
步骤八：对步骤七获取的一组三维图像进行标定，采用PointRCNN算法对标定的数据集进行训练，得到专用于检测畜牧舍目标的模型，当畜牧清洗自动驾驶车沿规划的路径行驶时，实时框取检测目标的三维框；
步骤九：通过步骤八获取的目标三维检测模型实时检测并识别前方挡板、工作人员等障碍物，框取检测物三维框，获取其三维信息(位置、大小、距离)，采用人工势场法通过三维信息规划处可通行局部路径，主控器控制小车的左转、右转、前进、后退、停止，实现行驶时的局部路径规划，保证小车无碰撞清洗；
步骤十：根据步骤八获取的清洗目标三维检测模型实时获取清洗对象(挡板、栅栏)的三维信息(位置、大小、距离)，匹配步骤三所得清洗对象3D模型，获取挡板、栅栏的准确清洗范围，通过PLC控制摆动电机和往复电机实现对清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
所述步骤一中采集方法具体为：将固态面阵激光雷达固定在畜牧清洗自动驾驶车上，车沿畜牧舍可通行路径行驶，采集畜牧舍内点云数据，并保存至数据bag中。
所述步骤二中使用体素化网格下采样处理步骤一获取的数据bag中的每一帧点云数据，对点云数据进行去噪滤波；
体素化网格下采样算法步骤为：根据点云数据创建一个最小三维体素栅格，然后计算出需要划分的小立方栅格的边长L，根据L的大小将三维体素栅格分解成m×n×1个小栅格，栅格划分完毕后将点云数据放到相应的小栅格中，同时删除那些不包含数据点的小栅格，在每个小栅格中，将离小栅格重心最近的数据点保留下来，删除其余的数据点，以此对点云数据进行去噪滤波，获取较为干净的畜牧舍点云数据；其计算步骤如下：
(1)小立方栅格的边长L的确定，小立方栅格的边长与邻近点个数k成正比，与点云的平均密度成反比，当点云的平均密度小时，表示在固定空间内的点云数量少，那么应将L取大些来提高k邻近搜索的范围，以保证有足够多点云进行k近邻的计算；当点云的平均密度大时，则表示在固定空间内的点云数量较多，应将L取小些，以保证在最适当的范围内搜索，降低搜索的时间；
小立方体栅格边长为如式1-1：
Figure BDA0003261845260000021
式中：α是比例因子，用来调节小立方体栅格的边长；s是比例系数；g是小栅格中点云数据的数目；
单位小栅格中包含的点云数据个数如式1-2：
n＝N/V (1-2)
式中：N表示点云数据中点的总数；V表示三维体素栅格的体积；
三维体素栅格的体积如式1-3：
V＝LxLyLz (1-3)
式中：Lx表示点云X轴方向最大范围；Lx表示点云Y轴方向最大范围；Lx表示点云Z轴方向最大范围，适当的将长方体向外扩大，增加距离λ对其修正，三维体素栅格三边的长度是：
Figure BDA0003261845260000031
将式(1-2)、(1-3)代入式(1-1)可得公式1-5：
Figure BDA0003261845260000032
(2)将点云数据划分到小立方体栅格中，根据小立方体栅格的边长L，将点云数据划分为m×n×l个小立方体栅格，其中，m＝ceil(Lx/l)，n＝ ceil(Ly/l)，l＝ceil(Lz/l)，ceil(x)为取整函数，表示不小于的最小整数，对于任一点pi，其所属小立方体栅格号如式1-6：
Figure BDA0003261845260000033
在整个栅格编码中，点Pi的栅格编码为
Figure BDA0003261845260000034
如果转换为一维编码的话，点Pi的栅格编码如式1-7所示：
Figure BDA0003261845260000035
根据式(1-6)或式(1-7)点云数据模型的每个数据点的栅格编码，且将编码放进哈希链表，建立点云数据间的空间拓扑关系后，确定出每个数据点Pi的邻近点，数据点Pi的邻近点是在它本身所在的小栅格和相邻小栅格中的数据点，而k 邻域的确定是计算这些数据点到点Pi的距离，并将按距离的远近，取出离Pi点最近的前k个数据点，存入到哈希链表中，作为数据点Pi的k邻域点，k取值范围一般为8～20；
(3)散乱点云法向量的估算，利用最小二乘平面法来估计数据点的法向量，假设物体的采样表面是足够光滑的，则采样得到的物体表面的数据点就可以用最小二乘平面法来实现曲面拟合，根据点云数据模型中的每个数据点Pi及其k近邻点，计算出每个数据点Pi及其k近邻点的最小二乘局部平面H，公式如1-8所示：
Figure RE-GDA0003521220490000036
式中：θ表示高斯权重；n表示最小二乘局部平面H的法向量，满足n2＝1；d表示最小二乘局部平面H与坐标原点两者之间的距离；
这样三维点云数据模型的法向量就可以用最小二乘局部平面的法向量近似表示；最小二乘局部平面平面H的法向量可以通过主成分分析得到，数据点Pi经过它的k邻域，如式1-9所示：
Figure BDA0003261845260000041
对式(1-9)分解，得到协方差矩阵M的3特征值λ0，λ1，λ2，设 0≤λ0≤λ1≤λ2，对应的3个特征向量分别是v0，v1，v2，则最小特征向量v0可以近似表示数据点Pi的法向量，协方差矩阵M如式1-10所示：
Figure BDA0003261845260000042
(4)代表点的选取及下采样实现，根据求出的点云数据模型中每个点Pi的邻域和点云数据模型中每个数据点Pi的法向量，求出每个数据点Pi的法向量与其邻近点法向之间的夹角，若法向量之间的夹角越大，则说明数据点Pi在其k 近邻附近的曲率变化比较大，存在尖锐的几何特征，相反，若法向量之间的夹角越小，则说明数据点Pi在其k近邻附近的曲率变化比较小，存在稀疏的几何特征，因此，可以预先设定一个阈值，当数据点Pi的法向量与其邻近点法向量之间的夹角大于阈值，则将该数据点视为特征点，调整边长比例因子α，使得立方体栅格边长变小，每个栅格点云数量变少，这样可以保留较多特征数据点，夹角小于阈值的点作为非特征点，调整边长比例因子α，使得立方体栅格变大，每个栅格点云数量变多，保留较少非特征数据点，每个小立方体栅格中，三维栅格中心集重心如式1-11所示：
Figure BDA0003261845260000043
式中g为小立方体栅格中点云数据个数，选取最靠近重心数据点保留下来，代表这个小立方体栅格所有数据点，实现点云下采样，完成对步骤一采集的点云数据进行滤波去噪，获取较为干净的畜牧舍点云数据。
所述步骤三中使用NDT和ICP融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3D模型；
首先根据步骤二所得点云数据得到网格参数大小，配准距离误差数据，其次进行NDT算法粗配准，，并对配准后的点云进行30％的抽稀，具体算法为：
(1)将点云数据划分为数个大小均匀的三维立方体，每个立方体中都包含有一定数量的点；
(2)点云在立方体中服从高斯分布，因此点云数据的高斯分布参数，如式1-12，1-13：
Figure BDA0003261845260000051
Figure BDA0003261845260000052
式中u表示均值向量，C表示协方差矩阵，n为立方体中总点数，xi为立方体中的一点，T表示转换矩阵；
(3)当一点落入小方格内部时，就根据内部的u和C对该点进行打分s，如式1-14：
Figure BDA0003261845260000053
(4)最小化(3)中的目标函数即可得到转换矩阵T，如式1-15：
Figure BDA0003261845260000054
(5)利用求得的变换矩阵对获取点云进行旋转变换，融合到相邻的点云数据中，并对配准后的点云进行30％的抽稀，得到一个粗配准点云结果；
然后进行KD树索引构建，搜索NDT粗配准点云的最近点，为ICP精细配准提供配准最近点，具体算法为：
(1)确定分割阈值：对所有的数据分别计算各个维度上数据的方差，选方差最大值作为所在维的分割域值；
(2)确定根节点：按照分割域值对三维数据进行排序，选中间点作为KD 树的根节点；
(3)确定左右子树空间：确定根节点后，使得所有小于根节点的点出现在左子树，剩余的点出现在右子树；
(4)分别对左右子树的点重复(2)和(3)，直至每个子空间都只有一个点，KD树是一种递归算法，当搜索相邻点时，首先确定搜索路径上最近点，然后回溯，检查有无最近点，将查询点和最近点的距离与最近点和分割平面的距离进行对比，前者大则进入该点的子空间，否则继续回溯直至前者大于后者，最终确定最近点；
最后通过ICP算法进行精细配准，得到畜牧舍清洗对象的3D模型，具体算法为：
(1)在点云B中取一点bi∈B；
(2)在点云A中找出与之对应的点ai∈A使得ai-bi＝min；
(3)计算出旋转矩阵R和平移矩阵t，使得误差函数最小；
(4)使用上步得出的旋转矩阵R和平移矩阵t对bi进行变换，得到新的点集，如式1-16：
b′i＝Rbi+t，bi∈B (1-16)
(5)计算b′i与点集A中与之对应的点ai的平均距离，如式 1-17：
b′i＝Rbi+t，bi∈B (1-17)
(6)若d小于设定的阈值或大于设定的迭代次数，则停止计算，否则返回步骤(2)，直到满足收敛条件为止。
所述步骤四中使用改进的Cartographer算法通过步骤二获得的处理过的点云数据进行配准建模，获取畜牧舍3D模型；
首先采用前端Local累加式建图，其主要步骤为：
(1)对一帧点云的每个点数据，筛出小于指定距离的点，并逐个记录为 returns；对于大于指定距离的点，将该方向上一定距离的某点记入misses点集。此处可将多帧数据一并存入并在后续工作中作一帧处理，默认仅累计一帧的数据；
(2)对输入的点云数据经过修剪筛去了z轴阈值之外的点，之后分别对 returns点集和misses点集进行体素滤波，每个体素内仅保留一个点；
(3)从姿态推算器中获取当前点云帧的姿态初值，并优化点云帧的位姿，匹配完成后把点云帧插入子图；
(4)将当前帧与较活跃的子图进行匹配以优化当前帧位姿，首先通过real-timeCSM(实时相关扫描匹配)对激光帧数据在submap的栅格地图上进行一次粗匹配，产生一个大体较好的位姿值并返回一个得分数；
然后采用后端Global位姿图构建与回环匹配，具体步骤为：
(1)根据前端建图的Local轨迹构建器和用于回环全局优化的位姿图，将每一帧的激光帧加入一个个子图，以完成实时累加式地图的建立，之后进行位姿图的构建，将匹配好的激光帧作为节点加入位姿图中进行后续全局匹配；
(2)通过稀疏位姿调整方法来优化所有激光雷达数据帧和子图的位姿，激光雷达数据帧在插入子图时的位姿会被缓存到内存中进行闭环检测，当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测，Cartographer算法使用稀疏位姿调整方法构建的优化问题的数学表达式为：
Figure BDA0003261845260000061
上式中，
Figure BDA0003261845260000071
分别表示在一定约束条件下，子图的位姿和扫描帧的位姿。相对位姿表示扫描帧在子图中的匹配位置，与其相关的协方差矩阵共同构成优化约束。这个约束的代价函数使用残差表示：
Figure BDA0003261845260000076
Figure BDA0003261845260000072
最后匹配出总的位姿图，即畜牧舍3D模型。
所述步骤五先根据畜牧舍3D模型确定畜牧舍入口和出口坐标，采用A*算法规划出清洗畜牧舍的可通行路径；
首先将畜牧舍环境M规定由栅格Mij构成：
Figure BDA0003261845260000073
其中，Mij＝0表示畜牧舍清洗驾驶车的起始位置单元栅格；Mij＝1表示无障碍区域单元栅格；Mij＝2表示障碍区域单元栅格；Mij＝3表示目标所处位置；
栅格大小的选取对于整个路径规划是很重要的，栅格选取较大，计算量就会减少，但得到的路径长度可能会增大；相反栅格选取太小，路径规划的准确度就会提高，但规划过程缓慢，栅格大小主要由畜牧舍环境确定。栅格长度：
Figure BDA0003261845260000074
其中，r为障碍物半径，R为畜牧舍清洗驾驶车半径，δ为设定的安全距离。综上可以将畜牧舍的路径规划问题概述为通过在已得到的环境模型中的无障碍区域搜索，得到一条由栅格组成的连续的最短路径；
其次A*算法根据所定义的估价函数大小来确定最优路径，代价函数表示为：
f(n)＝g(n)+h(n) (1- 24)
其中，n为当前节点；f(n)为节点n的估价函数；g(n)为从起始点到当前节点n的实际代价值；h(n)为当前点n到目标点的估计代价值。一般来说h(n)为欧氏距离，其定义如下：
Figure BDA0003261845260000075
其中，(xn，yn)表示当前位置处栅格的中心坐标；(xgoal，ygoal)则表示目标栅格的中心。通过优化可以得到当前位置到畜牧舍出口位置的最短路径。
所述步骤六中采用张正友算法对CMOS摄像头进行标定，获取摄像头参数矩阵；
张正友摄像头标定算法中像素坐标和世界坐标关系式如下所示。其中[R|T] 是外参矩阵，R是旋转矩阵，T是平移向量。左乘外参矩阵即可将世界坐标从世界坐标系转换到摄像机坐标系；
Figure BDA0003261845260000081
定的具体步骤为：
(1)制作黑白棋盘格，并固定在木板上；
(2)固定摄像头，拍摄25组不同角度位置的棋盘格板图像；
(3)将棋盘格图像导入Matlab的cameracalibrator工具中，并选定棋盘格板边长为49mm；
(4)工具自动标定棋盘格，获取摄像头各项参数。摄像头各项参数“RadiaDistortion”即为摄像头的畸变矩阵，“IntrinsicMatrix”为参数矩阵；
(5)根据摄像头参数矩阵和畸变参数矫正拍摄图像，以检测摄像头参数的准确性。
所述步骤七利用solvePnP算法通过步骤六得到的参数矩阵，计算CMOS摄像头到激光雷达的旋转矩阵和平移矩阵，获取到三维图像；
solvePnP算法是利用多个控制点(三维空间坐标以及对应图像平面坐标) 在三维场景中的坐标及其在图像中的透视投影坐标即可求解出摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R；
根据该算法先摄像头与雷达固定在畜牧清洗自动驾驶车上，并在车前2米处固定一块白板，用CMOS摄像头采集白板图像，且激光雷达采集相应点云数据，然后利用OpenCV获取鼠标左键点击位置图像上像素坐标(白板四角处)，同时通过CloudCompare点云工具获取点云数据相应位置的点云三维坐标。利用solvePnP算法通过所求的摄像头内参矩阵、畸变参数、图像二维坐标和点云三维坐标计算CMOS摄像头到激光雷达的旋转矩阵和平移矩阵。最后利用旋转矩阵和平移矩阵将点云数据投影到二维图像上，融合摄像头图像和激光雷达点云数据，获取到三维图像。
所述步骤八先标注三维图像，采用PointRCNN算法对标定的数据集进行训练，得到专用于检测畜牧舍目标的三维检测模型，行驶时实时框取畜牧舍目标的三维检测框；
首先需要对步骤六所得的三维图像进行三维标注，框选出图像中各类目标的三维检测框，同时也框选出相应点云数据里清洗对象的三维检测框，并采用 PointRCNN算法对标好的数据集进行训练，得到专用于畜牧舍目标的三维检测模型，其中PointRCNN算法训练检测模型分为两个阶段：
第一阶段，对点云进行语义分割，对每个点得到一个预测label，比如对所有判断是“车”的点(也叫做前景点)，赋予label＝1，其他点(也叫做背景点)，赋予label＝0。然后，用所有前景点生成boundingbox，一个前景点对应一个boundingbox，然后用了去除冗余的方法，继续减少boundingbox的数目，这一阶段结束的时候只留下300个boundingbox；
第二阶段，继续优化上一阶段生成的boundingbox。首先，对前一阶段生成的boundingbox做旋转平移，把这些boundingbox转换到自己的正规划坐标系下(canonicalcoordinates)。然后，通过点云池化等操作的到每个boundingbox的特征，再结合第一阶段的到的特征，进行boundingbox的修正和置信度的打分，从而的到最终的boundingbox。由此得到畜牧舍内清洗对象的三维检测模型。
所述步骤九中通过步骤八获取的畜牧舍目标三维检测模型实时检测障碍物，并获取其三维信息(位置、大小、距离)，根据获取的三维信息采用人工势场法规划出可通行局部路径，控制小车的左转、右转、前进、后退、停止，实现行驶时的局部路径规划，保证小车无碰撞清洗；
首先对控制器发送命令的串口进行测试，串口可正常发送数据时，根据步骤七获取的清洗对象检测模型实时检测挡板、工作人员等障碍物，并获取其位置、大小、距离三维信息，再根据获取的三维信息采用人工势场法规划出可通行局部路径，具体做法为：
(1)将畜牧清洗自动驾驶车、畜牧舍出口与障碍三者间的相对速度关系作为优化路径的指标。引力势场函数Uatt可定义为：
Figure BDA0003261845260000091
相应的引力Fatt可表示：
Fatt(q)＝mρ(q，qg)+kv1ρ(V，Vg) (1- 28)
式中，m、kv1为引力场正常量；ρ(q，qg)为畜牧清洗自动驾驶车与畜牧舍出口之间的欧氏距离；ρ(V，Vg)为畜牧清洗自动驾驶车和畜牧舍出口之间的相对速度；
斥力势场函数Urep可定义为：
Figure BDA0003261845260000092
Urepv＝kv2ρ2(V，Vobs)/2
(1-30)
Figure BDA0003261845260000101
相应斥力Frep可表示为：
Figure BDA0003261845260000102
Frepv＝kv2ρ(V，Vobs)
(1-33)
Figure BDA0003261845260000103
其中，k、kv2为斥力场正常量；ρ(q，qobs)表示畜牧清洗自动驾驶车与障碍物两点之间的欧氏距离；ρ(V，Vobs)表示畜牧清洗自动驾驶车和动态障碍物之间的相对速度；ρ0为常数，表示障碍物对畜牧清洗自动驾驶车产生作用的最大范围；α为障碍物的相对移动方向和移动畜牧清洗自动驾驶车与障碍物连线的夹角，当
Figure BDA0003261845260000104
时，障碍物的移动方向确定为靠近移动畜牧清洗自动驾驶车；
(2)当障碍物和畜牧舍出口对移动畜牧清洗自动驾驶车产生的排斥力和吸引力相等时，增设虚拟子目标法，借助虚拟外力，使畜牧清洗自动驾驶车摆脱陷阱，成功抵达出口。首先依据畜牧清洗自动驾驶车的状态信息判断是否进入局部最小值陷阱，以连续五个步长作为判断依据，当总的移动距离小于βL(L 为步长，β∈[2，4]时，改变β，提前检测到自身陷入局部极小值陷阱。其次程序跳转到虚拟子目标增设模块，存储使移动畜牧清洗自动驾驶车陷入局部最小值陷阱的障碍物位置信息，形成障碍物群，充分考虑出口位置与障碍物群位置信息，判断障碍物在移动畜牧清洗自动驾驶车与出口连接线左右两端数目，选择障碍物群左侧或右侧为目标障碍物，然后利用下式得到虚拟子目标位置：
Figure BDA0003261845260000105
式中，xob，yob为目标障碍物位置；(xg，yg)为原畜牧舍出口所处位置； (x，y)为畜牧清洗自动驾驶车的当前位置；β1，β2为可调参数，β1，β2＞0；移动畜牧清洗自动驾驶车摆脱上述陷阱后，将撤出虚拟子目标，移动畜牧清洗自动驾驶车不再受外力作用。移动畜牧清洗自动驾驶车将在原目标和障碍物的合力作用下向原畜牧舍出口靠近；
(3)当畜牧清洗自动驾驶车探测到的障碍物数目大于设定值或畜牧清洗自动驾驶车探测到障碍物与自身相对位置小于设定值时，环境即为复杂环境，在这种情况下，畜牧清洗自动驾驶车步长取Lmin；而当畜牧清洗自动驾驶车离目标很近时，为避免出现步长太大而越过过出口，或者步长太小而无法跟踪出口的情况，步长取Lmid；当畜牧清洗自动驾驶车处于简单环境时，步长取 Lmax。规划出一条局部路径；
随后上位机通过串口将路径信息发送给主控制器，主控器将上位机发来的路径信息通过串口中断进行接收，打包完成后然后通过CAN总通信将畜牧清洗自动驾驶车的前进、后退、刹车、左转和右转对应的控制命令发送到总线上，然后设置一个CAN总线发送标志位，当一个模块的数据打包完成后将标志位flag置1，然后通过CAN总线发送该模块的动作信息给从控制器；
从控制器通过CAN总线方式与主控器相连。从控制器包括两部分设计，一是CAN总线接收软件设计，二是使用PWM方式对各个舵机的脉冲宽度进行控制，实现畜牧清洗车的前进、后退、刹车和转向；
从控制器CAN总线接收设计采用STM32F103ZET6自带的一个bx_CAN 控制器、收发器是TJA1050芯片。从控制器上电后需要进行CAN总线部分的初始化，其中主要包括对从节点号、过滤器、波特率、中断的设置。从节点从 CAN总线上接收数据，然后CAN控制器对所接收数据进行过滤，得到对应各控制的数据信息；
从控制器PWM舵机舵角控制设计采用的是STM32F103ZET6，它具有两个 16位高级定时器TIM1、TIM8(带死区控制，有互补输出)和四个16位通用定时器TIM2～TIM5，均可输出PWM，产生DMA请求，以此完成畜牧清洗车的前进、后退、刹车和转向。
所述步骤十根据步骤八获取的畜牧舍目标三维检测模型实时获取清洗对象 (挡板、栅栏)的三维信息，匹配步骤三所得畜牧舍清洗对象3D模型，获取挡板、栅栏的准确清洗范围，通过PLC控制摆动电机和往复电机实现对清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗；
首先测试PLC串口是否连接成功，其次根据实时获取的挡板、栅栏的位置、大小、距离三维信息，匹配步骤三获取的畜牧舍清洗对象3D场景模型，以确定清洗目标的范围，若检测到栅栏时，通过PLC控制摆动电机控制清洗喷头纵向摆动，保证栅栏全方位清洗，若检测到挡板时，确定栅栏位置及大小形状信息，以锁定死角位置，随后通过PLC控制往复电机控制清洗杆上下摆动，达到死角反复清洗的目的，从而全方位清洗畜牧舍。
本发明的有益效果：
本发明一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法主要分为两大部分，自动驾驶系统和畜牧清洗系统。自动驾驶系统的信号采集是由超声波测距传感器、柔性薄膜压力传感器、九轴姿态传感器组成，通过采集到的信息结合激光雷达的采集的深度信息和机器视觉采集的图像信息，由主控制器控制两个舵机模块实现自动驾驶功能；畜牧清洗系统是根据激光雷达和机器视觉检测出清洗目标(栅栏、挡板)的三维目标信息，匹配该三维目标信息，控制器控制车速及清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
该发明可适用多种畜牧场景，普适性高，且融入自动驾驶技术达到高效率、成本低、省人力的效果，因此畜牧舍清洗自动驾驶车将会成为一个新的技术研究热点，带动畜牧业加快发展。
该发明可达到以下几点。
(1)畜牧舍可通行道路的自动规划及行驶，行驶无碰撞，规划路径保证可清洗到所有畜牧舍；
(2)畜牧舍围栏和死角清洗，清洗面积大于等于97％，地面残留有机物小于等于5％；
(3)雷达对畜牧舍场景实时建模；
(4)采用摄像机和激光雷达对畜牧舍内三维目标检测，实现畜牧舍内栅栏、挡板、工作人员等实时三维检测，实时检测准确率大于等于95％；
(5)建模、目标检测可实时显示；
(6)可实现自动驾驶和手动驾驶切换，自动手动切换时长小于等于3s。
附图说明
图1是本发明流程示意图。
图2是本发明实例提供的点云配准流程图。
图3是本发明实例提供的基于机器视觉的多目标识别算法设计流程图。
具体实施方式
下面结合附图对本发明作进一步详细说明。
如图1-图3所示：首先通过激光雷达采用改进的Cartographer算法对整个畜牧舍进行建模，得到3D畜牧舍模型，并根据模型采用A*和人工势场相结合的算法规划出清洗畜牧舍的可通行路径。其次采用solvePnP算法融合激光雷达采集的点云数据和摄像头采集的图像，并通过PointRCNN算法训练出专用于畜牧舍清洗对象的检测模型，畜牧清洗自动驾驶车沿规划的路径行驶时，根据激光雷达和机器视觉检测出清洗目标(栅栏、挡板)的三维目标信息，识别检测目标，并框取三维检测框。根据获取到的目标三维检测框匹配三维目标信息，利用主控制器控制两个舵机模块进行局部路径规划，同时依据清洗目标和清洗范围，然后从控制器控制车速及PLC控制清洗喷头的纵向摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗。
下面结合附图对本发明的应用原理作进一步的说明：
如图1所示为本发明方法的总体算法流程图，本发明所述的融合激光雷达和机器视觉的畜牧清洗自动驾驶车，按以下步骤进行：
步骤一：将固态面阵激光雷达固定在畜牧清洗自动驾驶车上，车沿畜牧舍可通行路径行驶，采集畜牧舍内点云数据，并保存至数据bag中。
步骤二：使用体素化网格下采样处理步骤一获取的数据bag中的每一帧点云数据，对点云数据进行去噪滤波；
体素化网格下采样算法步骤为：根据点云数据创建一个最小三维体素栅格，然后计算出需要划分的小立方栅格的边长L，根据L的大小将三维体素栅格分解成m×n×1个小栅格，栅格划分完毕后将点云数据放到相应的小栅格中，同时删除那些不包含数据点的小栅格，在每个小栅格中，将离小栅格重心最近的数据点保留下来，删除其余的数据点，以此对点云数据进行去噪滤波，获取较为干净的畜牧舍点云数据。其计算步骤如下：
(1)小立方栅格的边长L的确定。在体素化栅格下采样点云简化算法中，小立方体栅格边长L的选取十分重要：过大的栅格会降低搜索效率，过小的栅格则会出现空的栅格。小立方栅格的边长与邻近点个数k成正比，与点云的平均密度成反比，当点云的平均密度小时，表示在固定空间内的点云数量少，那么应将L取大些来提高k邻近搜索的范围，以保证有足够多点云进行k近邻的计算；当点云的平均密度大时，则表示在固定空间内的点云数量较多，应将L 取小些，以保证在最适当的范围内搜索，降低搜索的时间；
小立方体栅格边长为如式1-1：
Figure BDA0003261845260000131
式中：α是比例因子，用来调节小立方体栅格的边长；s是比例系数；g是小栅格中点云数据的数目；
单位小栅格中包含的点云数据个数如式1-2：
n＝N/V (1-2)
式中：N表示点云数据中点的总数；V表示三维体素栅格的体积；
三维体素栅格的体积如式1-3：
V＝LxLyLz (1-3)
式中：Lx表示点云X轴方向最大范围；Lx表示点云Y轴方向最大范围；Lx表示点云Z轴方向最大范围。而为了方便后续的统计，应确保数据点不会出现在外接三维体素栅格的角、边和面上。因此，适当的将长方体向外扩大，增加距离λ对其修正。所以，三维体素栅格三边的长度应该是：
Figure BDA0003261845260000132
将式(1-2)、(1-3)代入式(1-1)可得公式1-5：
Figure BDA0003261845260000133
(2)将点云数据划分到小立方体栅格中。根据小立方体栅格的边长L，将点云数据划分为m×n×l个小立方体栅格，其中，m＝ceil(Lx/l)，n＝ceil(Ly/l)，l＝ceil(Lz/l)。ceil(x)为取整函数，表示不小于的最小整数。对于任一点pi，其所属小立方体栅格号如式1-6：
Figure BDA0003261845260000141
在整个栅格编码中，点Pi的栅格编码为
Figure BDA0003261845260000142
如果转换为一维编码的话，点Pi的栅格编码如式1-7所示：
Figure BDA0003261845260000143
根据式(1-6)或式(1-7)点云数据模型的每个数据点的栅格编码，且将编码放进哈希链表，建立点云数据间的空间拓扑关系后，确定出每个数据点Pi的邻近点。数据点Pi的邻近点是在它本身所在的小栅格和相邻小栅格中的数据点。而k 邻域的确定是计算这些数据点到点Pi的距离，并将按距离的远近，取出离Pi点最近的前k个数据点，存入到哈希链表中，作为数据点Pi的k邻域点。k的取值，要综合考虑物体表面的凹凸性和点云数据模型的密度，并且要确保重建得到的曲面在每个数据点的邻近范围内是单凸性或者单凹性的，故k取值范围一般为8～20；
(3)散乱点云法向量的估算。利用最小二乘平面法来估计数据点的法向量，假设物体的采样表面是足够光滑的，则采样得到的物体表面的数据点就可以用最小二乘平面法来实现曲面拟合。根据点云数据模型中的每个数据点Pi及其k近邻点，计算出每个数据点Pi及其k近邻点的最小二乘局部平面H，公式如1-8所示：
Figure RE-GDA0003521220490000144
式中：θ表示高斯权重；n表示最小二乘局部平面H的法向量，满足n2＝1； d表示最小二乘局部平面H与坐标原点两者之间的距离；
这样三维点云数据模型的法向量就可以用最小二乘局部平面的法向量近似表示。最小二乘局部平面平面H的法向量可以通过主成分分析得到，数据点Pi经过它的k邻域，如式1-9所示：
Figure BDA0003261845260000145
对式(1-9)分解，得到协方差矩阵M的3特征值λ0，λ1，λ2，设 0≤λ0≤λ1≤λ2，对应的3个特征向量分别是v0，v1，v2，则最小特征向量v0可以近似表示数据点Pi的法向量，协方差矩阵M如式1-10所示：
Figure BDA0003261845260000146
(4)代表点的选取及下采样实现。根据求出的点云数据模型中每个点Pi的邻域和点云数据模型中每个数据点Pi的法向量，求出每个数据点Pi的法向量与其邻近点法向之间的夹角。若法向量之间的夹角越大，则说明数据点Pi在其k 近邻附近的曲率变化比较大，存在尖锐的几何特征。相反，若法向量之间的夹角越小，则说明数据点Pi在其k近邻附近的曲率变化比较小，存在稀疏的几何特征。因此，可以预先设定一个阈值，当数据点Pi的法向量与其邻近点法向量之间的夹角大于阈值，则将该数据点视为特征点，调整边长比例因子α，使得立方体栅格边长变小，每个栅格点云数量变少，这样可以保留较多特征数据点。夹角小于阈值的点作为非特征点，调整边长比例因子α，使得立方体栅格变大，每个栅格点云数量变多，保留较少非特征数据点。每个小立方体栅格中，三维栅格中心集重心如式1-11所示：
Figure BDA0003261845260000151
式中g为小立方体栅格中点云数据个数。选取最靠近重心数据点保留下来，代表这个小立方体栅格所有数据点，实现点云下采样。完成对步骤一采集的点云数据进行滤波去噪，获取较为干净的畜牧舍点云数据。
步骤三：使用NDT和ICP融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3D模型；
首先根据步骤二所得点云数据得到网格参数大小，配准距离误差数据，其次进行NDT算法粗配准，，并对配准后的点云进行30％的抽稀，具体算法为：
(1)将点云数据划分为数个大小均匀的三维立方体，每个立方体中都包含有一定数量的点；
(2)点云在立方体中服从高斯分布，因此点云数据的高斯分布参数，如式 1-12，1-13：
Figure BDA0003261845260000152
Figure BDA0003261845260000153
式中u表示均值向量，C表示协方差矩阵，n为立方体中总点数，xi为立方体中的一点，T表示转换矩阵；
(3)当一点落入小方格内部时，就根据内部的u和C对该点进行打分s，如式1-14：
Figure BDA0003261845260000161
(4)最小化(3)中的目标函数即可得到转换矩阵T，如式1-15：
Figure BDA0003261845260000162
(5)利用求得的变换矩阵对获取点云进行旋转变换，融合到相邻的点云数据中，并对配准后的点云进行30％的抽稀，得到一个粗配准点云结果。
然后进行KD树索引构建，搜索NDT粗配准点云的最近点，为ICP精细配准提供配准最近点，具体算法为：
(1)确定分割阈值：对所有的数据分别计算各个维度上数据的方差，选方差最大值作为所在维的分割域值；
(2)确定根节点：按照分割域值对三维数据进行排序，选中间点作为KD 树的根节点；
(3)确定左右子树空间：确定根节点后，使得所有小于根节点的点出现在左子树，剩余的点出现在右子树；
(4)分别对左右子树的点重复(2)和(3)，直至每个子空间都只有一个点。KD树是一种递归算法，当搜索相邻点时，首先确定搜索路径上最近点，然后回溯，检查有无最近点，将查询点和最近点的距离与最近点和分割平面的距离进行对比，前者大则进入该点的子空间，否则继续回溯直至前者大于后者，最终确定最近点。
最后通过ICP算法进行精细配准，得到畜牧舍清洗对象的3D模型，具体算法为：
(1)在点云B中取一点bi∈B；
(2)在点云A中找出与之对应的点ai∈A使得ai-bi＝min；
(3)计算出旋转矩阵R和平移矩阵t，使得误差函数最小；
(4)使用上步得出的旋转矩阵R和平移矩阵t对bi进行变换，得到新的点集，如式1-16：
b′i＝Rbi+t，bi∈B (1-16)
(5)计算b′i与点集A中与之对应的点ai的平均距离，如式 1-17：
b′i＝Rbi+t，bi∈B (1-17)
(6)若d小于设定的阈值或大于设定的迭代次数，则停止计算，否则返回步骤(2)，直到满足收敛条件为止。
步骤四：使用改进的Cartographer算法通过步骤二获得的处理过的点云数据进行配准建模，获取畜牧舍3D模型；
首先采用前端Local累加式建图，其主要步骤为：
(1)对一帧点云的每个点数据，筛出小于指定距离的点，并逐个记录为 returns；对于大于指定距离的点，将该方向上一定距离的某点记入misses点集。此处可将多帧数据一并存入并在后续工作中作一帧处理，默认仅累计一帧的数据；
(2)对输入的点云数据经过修剪筛去了z轴阈值之外的点。之后分别对 returns点集和misses点集进行体素滤波。每个体素内仅保留一个点；
(3)从姿态推算器中获取当前点云帧的姿态初值，并优化点云帧的位姿。匹配完成后把点云帧插入子图；
(4)将当前帧与较活跃的子图进行匹配以优化当前帧位姿，首先通过real-timeCSM(实时相关扫描匹配)对激光帧数据在submap的栅格地图上进行一次粗匹配，产生一个大体较好的位姿值并返回一个得分数。
然后采用后端Global位姿图构建与回环匹配，具体步骤为：
(1)根据前端建图的Local轨迹构建器和用于回环全局优化的位姿图，将每一帧的激光帧加入一个个子图，以完成实时累加式地图的建立，之后进行位姿图的构建，将匹配好的激光帧作为节点加入位姿图中进行后续全局匹配；
(2)通过稀疏位姿调整方法来优化所有激光雷达数据帧和子图的位姿，激光雷达数据帧在插入子图时的位姿会被缓存到内存中进行闭环检测。当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测。Cartographer算法使用稀疏位姿调整方法构建的优化问题的数学表达式为：
Figure BDA0003261845260000171
上式中，
Figure BDA0003261845260000172
分别表示在一定约束条件下，子图的位姿和扫描帧的位姿。相对位姿表示扫描帧在子图中的匹配位置，与其相关的协方差矩阵共同构成优化约束。这个约束的代价函数使用残差表示：
Figure BDA0003261845260000173
Figure BDA0003261845260000174
最后匹配出总的位姿图，即畜牧舍3D模型。
步骤五：根据畜牧舍3D模型，确定畜牧舍入口和出口坐标，采用A*算法规划出清洗畜牧舍挡板、围栏的可通行路径；
首先将畜牧舍环境M规定由栅格Mij构成：
M＝{Mij，Mij＝0，1，2，3} (1- 22)
其中，Mij＝0表示畜牧舍清洗驾驶车的起始位置单元栅格；Mij＝1表示无障碍区域单元栅格；Mij＝2表示障碍区域单元栅格；Mij＝3表示目标所处位置。
栅格大小的选取对于整个路径规划是很重要的，栅格选取较大，计算量就会减少，但得到的路径长度可能会增大；相反栅格选取太小，路径规划的准确度就会提高，但规划过程缓慢。栅格大小主要由畜牧舍环境确定。栅格长度：
Figure BDA0003261845260000181
其中，r为障碍物半径，R为畜牧舍清洗驾驶车半径，δ为设定的安全距离。综上可以将畜牧舍的路径规划问题概述为通过在已得到的环境模型中的无障碍区域搜索，得到一条由栅格组成的连续的最短路径；
其次A*算法根据所定义的估价函数大小来确定最优路径。代价函数表示为：
f(n)＝g(n)+h(n) (1- 24)
其中，n为当前节点；f(n)为节点n的估价函数；g(n)为从起始点到当前节点n的实际代价值；h(n)为当前点n到目标点的估计代价值。一般来说h(n)为欧氏距离，其定义如下：
Figure BDA0003261845260000182
其中，(xn，yn)表示当前位置处栅格的中心坐标；(xgoal，ygoal)则表示目标栅格的中心。通过优化可以得到当前位置到畜牧舍出口位置的最短路径。
步骤六：采用张正友算法对CMOS摄像头进行标定，获取摄像头参数矩阵；
张正友摄像头标定算法中像素坐标和世界坐标关系式如下所示。其中[R|T] 是外参矩阵，R是旋转矩阵，T是平移向量。左乘外参矩阵即可将世界坐标从世界坐标系转换到摄像机坐标系。
Figure BDA0003261845260000191
标定的具体步骤为：
(1)制作黑白棋盘格，并固定在木板上；
(2)固定摄像头，拍摄25组不同角度位置的棋盘格板图像；
(3)将棋盘格图像导入Matlab的cameracalibrator工具中，并选定棋盘格板边长为49mm；
(4)工具自动标定棋盘格，获取摄像头各项参数。摄像头各项参数中“RadiaDistortion”即为摄像头的畸变矩阵，“IntrinsicMatrix”为参数矩阵；
(5)根据摄像头参数矩阵和畸变参数矫正拍摄图像，以检测摄像头参数的准确性。
步骤七：利用solvePnP算法通过步骤六得到的参数矩阵，计算CMOS摄像头到激光雷达的旋转矩阵和平移矩阵，获取到三维图像；
solvePnP算法是利用多个控制点(三维空间坐标以及对应图像平面坐标) 在三维场景中的坐标及其在图像中的透视投影坐标即可求解出摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R；
根据该算法先摄像头与雷达固定在畜牧清洗自动驾驶车上，并在车前2米处固定一块白板，用CMOS摄像头采集白板图像，且激光雷达采集相应点云数据，然后利用OpenCV获取鼠标左键点击位置图像上像素坐标(白板四角处)，同时通过CloudCompare点云工具获取点云数据相应位置的点云三维坐标。利用solvePnP算法通过所求的摄像头内参矩阵、畸变参数、图像二维坐标和点云三维坐标计算CMOS摄像头到激光雷达的旋转矩阵和平移矩阵。最后利用旋转矩阵和平移矩阵将点云数据投影到二维图像上，融合摄像头图像和激光雷达点云数据，获取到三维图像。
步骤八：标注三维图像，采用PointRCNN算法对标定的数据集进行训练，得到专用于检测畜牧舍目标的三维检测模型，行驶时实时框取畜牧舍目标的三维检测框；
首先需要对步骤六所得的三维图像进行三维标注，框选出图像中各类目标的三维检测框，同时也框选出相应点云数据里清洗对象的三维检测框，并采用 PointRCNN算法对标好的数据集进行训练，得到专用于畜牧舍目标的三维检测模型，其中PointRCNN算法训练检测模型分为两个阶段：
第一阶段，对点云进行语义分割，对每个点得到一个预测label，比如对所有判断是“车”的点(也叫做前景点)，赋予label＝1，其他点(也叫做背景点)，赋予label＝0。然后，用所有前景点生成boundingbox，一个前景点对应一个boundingbox，然后用了去除冗余的方法，继续减少boundingbox的数目，这一阶段结束的时候只留下300个boundingbox；
第二阶段，继续优化上一阶段生成的boundingbox。首先，对前一阶段生成的boundingbox做旋转平移，把这些boundingbox转换到自己的正规划坐标系下(canonicalcoordinates)。然后，通过点云池化等操作的到每个boundingbox的特征，再结合第一阶段的到的特征，进行boundingbox的修正和置信度的打分，从而的到最终的boundingbox。由此得到畜牧舍内清洗对象的三维检测模型。
步骤九：通过步骤八获取的畜牧舍目标三维检测模型实时检测障碍物，并获取其三维信息(位置、大小、距离)，根据获取的三维信息控制小车的左转、右转、前进、后退、停止，实现行驶时的局部路径规划，保证小车无碰撞清洗；
首先对控制器发送命令的串口进行测试，串口可正常发送数据时，根据步骤七获取的清洗对象检测模型实时检测挡板、工作人员等障碍物，并获取其位置、大小、距离三维信息，再根据获取的三维信息采用人工势场法规划出可通行局部路径，具体做法为：
(1)将畜牧清洗自动驾驶车、畜牧舍出口与障碍三者间的相对速度关系作为优化路径的指标。引力势场函数Uatt可定义为：
Uatt(q)＝[mρ2(q，qg)+kv1ρ2(V，Vg)]/2 (1- 17)
相应的引力Fatt可表示：
Fatt(q)＝mρ(q，qg)+kv1ρ(X，Vg)
(1-28)
式中，m、kv1为引力场正常量；ρ(q，qg)为畜牧清洗自动驾驶车与畜牧舍出口之间的欧氏距离；ρ(V，Vg)为畜牧清洗自动驾驶车和畜牧舍出口之间的相对速度；
斥力势场函数Urep可定义为：
Figure BDA0003261845260000201
Urepv＝kv2ρ2(V，Vobs)/2
(1-30)
Figure BDA0003261845260000202
相应斥力Frep可表示为：
Figure BDA0003261845260000211
Frepv＝kv2ρ(V，Vobs)
(1-33)
Figure BDA0003261845260000212
其中，k、kv2为斥力场正常量；ρ(q，qobs)表示畜牧清洗自动驾驶车与障碍物两点之间的欧氏距离；ρ(V，Vobs)表示畜牧清洗自动驾驶车和动态障碍物之间的相对速度；ρ0为常数，表示障碍物对畜牧清洗自动驾驶车产生作用的最大范围；α为障碍物的相对移动方向和移动畜牧清洗自动驾驶车与障碍物连线的夹角，当
Figure BDA0003261845260000213
时，障碍物的移动方向确定为靠近移动畜牧清洗自动驾驶车；
(2)当障碍物和畜牧舍出口对移动畜牧清洗自动驾驶车产生的排斥力和吸引力相等时，增设虚拟子目标法，借助虚拟外力，使畜牧清洗自动驾驶车摆脱陷阱，成功抵达出口。首先依据畜牧清洗自动驾驶车的状态信息判断是否进入局部最小值陷阱，以连续五个步长作为判断依据，当总的移动距离小于βL(L 为步长，β∈[2，4]时，改变β，提前检测到自身陷入局部极小值陷阱。其次程序跳转到虚拟子目标增设模块，存储使移动畜牧清洗自动驾驶车陷入局部最小值陷阱的障碍物位置信息，形成障碍物群，充分考虑出口位置与障碍物群位置信息，判断障碍物在移动畜牧清洗自动驾驶车与出口连接线左右两端数目，选择障碍物群左侧或右侧为目标障碍物，然后利用下式得到虚拟子目标位置：
Figure BDA0003261845260000214
式中，xob，yob为目标障碍物位置；(xg，yg)为原畜牧舍出口所处位置； (x，y)为畜牧清洗自动驾驶车的当前位置；β1，β2为可调参数，β1，β2＞0；移动畜牧清洗自动驾驶车摆脱上述陷阱后，将撤出虚拟子目标，移动畜牧清洗自动驾驶车不再受外力作用。移动畜牧清洗自动驾驶车将在原目标和障碍物的合力作用下向原畜牧舍出口靠近；
(3)当畜牧清洗自动驾驶车探测到的障碍物数目大于设定值或畜牧清洗自动驾驶车探测到障碍物与自身相对位置小于设定值时，环境即为复杂环境。在这种情况下，畜牧清洗自动驾驶车步长取Lmin；而当畜牧清洗自动驾驶车离目标很近时，为避免出现步长太大而越过过出口，或者步长太小而无法跟踪出口的情况，步长取Lmid；当畜牧清洗自动驾驶车处于简单环境时，步长取Lmax。规划出一条局部路径。
随后上位机通过串口将路径信息发送给主控制器，主控器将上位机发来的路径信息通过串口中断进行接收，打包完成后然后通过CAN总通信将畜牧清洗自动驾驶车的前进、后退、刹车、左转和右转对应的控制命令发送到总线上，然后设置一个CAN总线发送标志位，当一个模块的数据打包完成后将标志位flag置1，然后通过CAN总线发送该模块的动作信息给从控制器；
从控制器通过CAN总线方式与主控器相连。从控制器包括两部分设计，一是CAN总线接收软件设计，二是使用PWM方式对各个舵机的脉冲宽度进行控制，实现畜牧清洗车的前进、后退、刹车和转向；
从控制器CAN总线接收设计采用STM32F103ZET6自带的一个bx_CAN 控制器、收发器是TJA1050芯片。从控制器上电后需要进行CAN总线部分的初始化，其中主要包括对从节点号、过滤器、波特率、中断的设置。从节点从 CAN总线上接收数据，然后CAN控制器对所接收数据进行过滤，得到对应各控制的数据信息；
从控制器PWM舵机舵角控制设计采用的是STM32F103ZET6，它具有两个 16位高级定时器TIM1、TIM8(带死区控制，有互补输出)和四个16位通用定时器TIM2～TIM5，均可输出PWM，产生DMA请求，以此完成畜牧清洗车的前进、后退、刹车和转向。
步骤十：根据步骤八获取的畜牧舍目标三维检测模型实时获取清洗对象 (挡板、栅栏)的三维信息，匹配步骤三所得畜牧舍清洗对象3D模型，获取挡板、栅栏的准确清洗范围，通过PLC控制摆动电机和往复电机实现对清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗；
首先测试PLC串口是否连接成功，其次根据实时获取的挡板、栅栏的位置、大小、距离三维信息，匹配步骤三获取的畜牧舍清洗对象3D场景模型，以确定清洗目标的范围，若检测到栅栏时，通过PLC控制摆动电机控制清洗喷头纵向摆动，保证栅栏全方位清洗，若检测到挡板时，确定栅栏位置及大小形状信息，以锁定死角位置，随后通过PLC控制往复电机控制清洗杆上下摆动，达到死角反复清洗的目的，从而全方位清洗畜牧舍。

Claims (10)

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：
Figure RE-FDA0003521220480000021
式中:α是比例因子，用来调节小立方体栅格的边长；s是比例系数；g是小栅格中点云数据的数目；
单位小栅格中包含的点云数据个数如式1-2：
n＝N/V (1-2)
式中:N表示点云数据中点的总数；V表示三维体素栅格的体积；
三维体素栅格的体积如式1-3：
V＝LxLyLz (1-3)
式中:Lx表示点云X轴方向最大范围；Lx表示点云Y轴方向最大范围；Lx表示点云Z轴方向最大范围，适当的将长方体向外扩大，增加距离λ对其修正，所以，三维体素栅格三边的长度是：
Figure RE-FDA0003521220480000022
将式(1-2)、(1-3)代入式(1-1)可得公式1-5：
Figure RE-FDA0003521220480000023
(2)将点云数据划分到小立方体栅格中，根据小立方体栅格的边长L，将点云数据划分为m×n×l个小立方体栅格，其中，m＝ceil(Lx/l)，n＝ceil(Ly/l)，l＝ceil(Lz/l)，ceil(x)为取整函数，表示不小于的最小整数。对于任一点pi，其所属小立方体栅格号如式1-6：
Figure RE-FDA0003521220480000024
在整个栅格编码中，点Pi的栅格编码为
Figure RE-FDA0003521220480000025
如果转换为一维编码的话，点Pi的栅格编码如式1-7所示：
Figure RE-FDA0003521220480000026
根据式(1-6)或式(1-7)点云数据模型的每个数据点的栅格编码，且将编码放进哈希链表，建立点云数据间的空间拓扑关系后，确定出每个数据点Pi的邻近点，数据点Pi的邻近点是在它本身所在的小栅格和相邻小栅格中的数据点，而k 邻域的确定是计算这些数据点到点Pi的距离，并将按距离的远近，取出离Pi点最近的前k个数据点，存入到哈希链表中，作为数据点Pi的k邻域点，k的取值，要综合考虑物体表面的凹凸性和点云数据模型的密度，并且要确保重建得到的曲面在每个数据点的邻近范围内是单凸性或者单凹性的，故k取值范围一般为8～20；
(3)散乱点云法向量的估算，利用最小二乘平面法来估计数据点的法向量，假设物体的采样表面是足够光滑的，则采样得到的物体表面的数据点就可以用最小二乘平面法来实现曲面拟合，根据点云数据模型中的每个数据点Pi及其k近邻点，计算出每个数据点Pi及其k近邻点的最小二乘局部平面H，公式如1-8所示：
Figure RE-FDA0003521220480000031
式中:θ表示高斯权重；n表示最小二乘局部平面H的法向量，满足n2＝1；d表示最小二乘局部平面H与坐标原点两者之间的距离；
最小二乘局部平面平面H的法向量可以通过主成分分析得到，数据点Pi经过它的k邻域，如式1-9所示：
Figure RE-FDA0003521220480000032
对式(1-9)分解，得到协方差矩阵M的3特征值λ0，λ1，λ2，设0≤λ0≤λ1≤λ2，对应的3个特征向量分别是v0，v1，v2，则最小特征向量v0可以近似表示数据点Pi的法向量，协方差矩阵M如式1-10所示：
Figure RE-FDA0003521220480000033
(4)代表点的选取及下采样实现，根据求出的点云数据模型中每个点Pi的邻域和点云数据模型中每个数据点Pi的法向量，求出每个数据点Pi的法向量与其邻近点法向之间的夹角，若法向量之间的夹角越大，则说明数据点Pi在其k近邻附近的曲率变化比较大，存在尖锐的几何特征，相反，若法向量之间的夹角越小，则说明数据点Pi在其k近邻附近的曲率变化比较小，存在稀疏的几何特征，预先设定一个阈值，当数据点Pi的法向量与其邻近点法向量之间的夹角大于阈值，则将该数据点视为特征点，调整边长比例因子α，使得立方体栅格边长变小，每个栅格点云数量变少，这样保留较多特征数据点，夹角小于阈值的点作为非特征点，调整边长比例因子α，使得立方体栅格变大，每个栅格点云数量变多，保留较少非特征数据点，每个小立方体栅格中，三维栅格中心集重心如式1-11所示：
Figure RE-FDA0003521220480000041
式中g为小立方体栅格中点云数据个数。选取最靠近重心数据点保留下来，代表这个小立方体栅格所有数据点，实现点云下采样。完成对步骤一采集的点云数据进行滤波去噪，获取较为干净的畜牧舍点云数据。
4.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤三中使用NDT和ICP融合的算法对步骤二得有关清洗对象的点云数据进行配准，获取畜牧舍内清洗对象3D模型；
首先根据步骤二所得点云数据得到网格参数大小，配准距离误差数据，其次进行NDT算法粗配准，，并对配准后的点云进行30％的抽稀，具体算法为：
(1)将点云数据划分为数个大小均匀的三维立方体，每个立方体中都包含有一定数量的点；
(2)点云在立方体中服从高斯分布，因此点云数据的高斯分布参数，如式1-12，1-13：
Figure RE-FDA0003521220480000042
Figure RE-FDA0003521220480000043
式中u表示均值向量，C表示协方差矩阵，n为立方体中总点数，xi为立方体中的一点，T表示转换矩阵；
(3)当一点落入小方格内部时，就根据内部的u和C对该点进行打分s，如式1-14：
Figure RE-FDA0003521220480000044
(4)最小化(3)中的目标函数即可得到转换矩阵T，如式1-15：
Figure RE-FDA0003521220480000045
(5)利用求得的变换矩阵对获取点云进行旋转变换，融合到相邻的点云数据中，并对配准后的点云进行30％的抽稀，得到一个粗配准点云结果；
然后进行KD树索引构建，搜索NDT粗配准点云的最近点，为ICP精细配准提供配准最近点，具体算法为：
(1)确定分割阈值：对所有的数据分别计算各个维度上数据的方差，选方差最大值作为所在维的分割域值；
(2)确定根节点：按照分割域值对三维数据进行排序，选中间点作为KD树的根节点；
(3)确定左右子树空间：确定根节点后，使得所有小于根节点的点出现在左子树，剩余的点出现在右子树；
(4)分别对左右子树的点重复(2)和(3)，直至每个子空间都只有一个点，当搜索相邻点时，首先确定搜索路径上最近点，然后回溯，检查有无最近点，将查询点和最近点的距离与最近点和分割平面的距离进行对比，前者大则进入该点的子空间，否则继续回溯直至前者大于后者，最终确定最近点；
最后通过ICP算法进行精细配准，得到畜牧舍清洗对象的3D模型，具体算法为：
(1)在点云B中取一点bi∈B；
(2)在点云A中找出与之对应的点ai∈A使得ai－bi＝min；
(3)计算出旋转矩阵R和平移矩阵t，使得误差函数最小；
(4)使用上步得出的旋转矩阵R和平移矩阵t对bi进行变换，得到新的点集，如式1-16:
b′i＝Rbi+t,bi∈B (1-16)
(5)计算b′i与点集A中与之对应的点ai的平均距离，如式1-17:
b′i＝Rbi+t,bi∈B (1-17)
(6)若d小于设定的阈值或大于设定的迭代次数，则停止计算，否则返回步骤(2)，直到满足收敛条件为止。
5.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤四中使用改进的Cartographer算法通过步骤二获得的处理过的点云数据进行配准建模，获取畜牧舍3D模型；
首先采用前端Local累加式建图，其主要步骤为：
(1)对一帧点云的每个点数据，筛出小于指定距离的点，并逐个记录为returns；对于大于指定距离的点，将该方向上一定距离的某点记入misses点集。此处可将多帧数据一并存入并在后续工作中作一帧处理，默认仅累计一帧的数据；
(2)对输入的点云数据经过修剪筛去了z轴阈值之外的点。之后分别对returns点集和misses点集进行体素滤波。每个体素内仅保留一个点；
(3)从姿态推算器中获取当前点云帧的姿态初值，并优化点云帧的位姿，匹配完成后把点云帧插入子图；
(4)将当前帧与较活跃的子图进行匹配以优化当前帧位姿，首先通过real-timeCSM对激光帧数据在submap的栅格地图上进行一次粗匹配，产生一个大体较好的位姿值并返回一个得分数；
然后采用后端Global位姿图构建与回环匹配，具体步骤为：
(1)根据前端建图的Local轨迹构建器和用于回环全局优化的位姿图，将每一帧的激光帧加入一个个子图，以完成实时累加式地图的建立，之后进行位姿图的构建，将匹配好的激光帧作为节点加入位姿图中进行后续全局匹配；
(2)通过稀疏位姿调整方法来优化所有激光雷达数据帧和子图的位姿，激光雷达数据帧在插入子图时的位姿会被缓存到内存中进行闭环检测，当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测。Cartographer算法使用稀疏位姿调整方法构建的优化问题的数学表达式为：
Figure RE-FDA0003521220480000061
上式中，
Figure RE-FDA0003521220480000062
分别表示在一定约束条件下，子图的位姿和扫描帧的位姿，相对位姿表示扫描帧在子图中的匹配位置，与其相关的协方差矩阵共同构成优化约束，这个约束的代价函数使用残差表示：
Figure RE-FDA0003521220480000063
Figure RE-FDA0003521220480000064
最后匹配出总的位姿图，即畜牧舍3D模型。
6.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤五先根据畜牧舍3D模型确定畜牧舍入口和出口坐标，采用A*算法规划出清洗畜牧舍的可通行路径；
首先将畜牧舍环境M规定由栅格Mij构成：
M＝{Mij,Mij＝0,1,2,3}
(1-22)
其中，Mij＝0表示畜牧舍清洗驾驶车的起始位置单元栅格；Mij＝1表示无障碍区域单元栅格；Mij＝2表示障碍区域单元栅格；Mij＝3表示目标所处位置；
栅格长度：
Figure RE-FDA0003521220480000071
其中，r为障碍物半径，R为畜牧舍清洗驾驶车半径，δ为设定的安全距离，综上可以将畜牧舍的路径规划问题概述为通过在已得到的环境模型中的无障碍区域搜索，得到一条由栅格组成的连续的最短路径；
其次A*算法根据所定义的估价函数大小来确定最优路径。代价函数表示为：
f(n)＝g(n)+h(n) (1-24)
其中，n为当前节点；f(n)为节点n的估价函数；g(n)为从起始点到当前节点n的实际代价值；h(n)为当前点n到目标点的估计代价值。一般来说h(n)为欧氏距离，其定义如下：
Figure RE-FDA0003521220480000072
其中，(xn,yn)表示当前位置处栅格的中心坐标；(xgoal,ygoal)则表示目标栅格的中心，通过优化可以得到当前位置到畜牧舍出口位置的最短路径。
7.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤六中采用张正友算法对CMOS摄像头进行标定，获取摄像头参数矩阵；
张正友摄像头标定算法中像素坐标和世界坐标关系式如下所示。其中[R|T]是外参矩阵，R是旋转矩阵，T是平移向量。左乘外参矩阵即可将世界坐标从世界坐标系转换到摄像机坐标系；
Figure RE-FDA0003521220480000073
标定的具体步骤为：
(1)制作黑白棋盘格，并固定在木板上；
(2)固定摄像头，拍摄25组不同角度位置的棋盘格板图像；
(3)将棋盘格图像导入Matlab的cameracalibrator工具中，并选定棋盘格板边长为49mm；
(4)工具自动标定棋盘格，获取摄像头各项参数。摄像头各项参数中“RadiaDistortion”即为摄像头的畸变矩阵，“IntrinsicMatrix”为参数矩阵；
(5)根据摄像头参数矩阵和畸变参数矫正拍摄图像，以检测摄像头参数的准确性。
8.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤七利用solvePnP算法通过步骤六得到的参数矩阵，计算CMOS摄像头到激光雷达的旋转矩阵和平移矩阵，获取到三维图像；
solvePnP算法是利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标即可求解出摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R；
根据该算法先摄像头与雷达固定在畜牧清洗自动驾驶车上，并在车前2米处固定一块白板，用CMOS摄像头采集白板图像，且激光雷达采集相应点云数据，然后利用OpenCV获取鼠标左键点击位置图像上像素坐标，同时通过CloudCompare点云工具获取点云数据相应位置的点云三维坐标，利用solvePnP算法通过所求的摄像头内参矩阵、畸变参数、图像二维坐标和点云三维坐标计算CMOS摄像头到激光雷达的旋转矩阵和平移矩阵。最后利用旋转矩阵和平移矩阵将点云数据投影到二维图像上，融合摄像头图像和激光雷达点云数据，获取到三维图像。
9.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤八先标注三维图像，采用PointRCNN算法对标定的数据集进行训练，得到专用于检测畜牧舍目标的三维检测模型，行驶时实时框取畜牧舍目标的三维检测框；
首先需要对步骤六所得的三维图像进行三维标注，框选出图像中各类目标的三维检测框，同时也框选出相应点云数据里清洗对象的三维检测框，并采用PointRCNN算法对标好的数据集进行训练，得到专用于畜牧舍目标的三维检测模型，其中PointRCNN算法训练检测模型分为两个阶段：
第一阶段，对点云进行语义分割，对每个点得到一个预测label，比如对所有判断是“车”的点，赋予label＝1，其他点(也叫做背景点)，赋予label＝0。然后，用所有前景点生成boundingbox，一个前景点对应一个boundingbox，然后用了去除冗余的方法，继续减少boundingbox的数目，这一阶段结束的时候只留下300个boundingbox；
第二阶段，继续优化上一阶段生成的boundingbox。首先，对前一阶段生成的boundingbox做旋转平移，把这些boundingbox转换到自己的正规划坐标系下(canonicalcoordinates)，然后，通过点云池化等操作的到每个boundingbox的特征，再结合第一阶段的到的特征，进行boundingbox的修正和置信度的打分，从而的到最终的boundingbox。由此得到畜牧舍内清洗对象的三维检测模型。
10.根据权利要求1所述的一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法，其特征在于，所述步骤九中通过步骤八获取的畜牧舍目标三维检测模型实时检测障碍物，并获取其三维信息，根据获取的三维信息采用人工势场法规划出可通行局部路径，控制小车的左转、右转、前进、后退、停止，实现行驶时的局部路径规划，保证小车无碰撞清洗；
首先对控制器发送命令的串口进行测试，串口可正常发送数据时，根据步骤七获取的清洗对象检测模型实时检测挡板、工作人员等障碍物，并获取其位置、大小、距离三维信息，再根据获取的三维信息采用人工势场法规划出可通行局部路径，具体做法为：
(1)将畜牧清洗自动驾驶车、畜牧舍出口与障碍三者间的相对速度关系作为优化路径的指标。引力势场函数Uatt可定义为：
Uatt(q)＝[mρ2(q,qg)+kv1ρ2(V,Vg)]/2 (1-27)
相应的引力Fatt可表示：
Fatt(q)＝mρ(q,qg)+kv1ρ(V,Vg)
(1-28)
式中，m、kv1为引力场正常量；ρ(q,qg)为畜牧清洗自动驾驶车与畜牧舍出口之间的欧氏距离；ρ(V,Vg)为畜牧清洗自动驾驶车和畜牧舍出口之间的相对速度；
斥力势场函数Urep可定义为：
Figure RE-FDA0003521220480000091
Urepv＝kv2ρ2(V,Vobs)/2
(1-30)
Figure RE-FDA0003521220480000092
相应斥力Frep可表示为：
Figure RE-FDA0003521220480000093
Frepv＝kv2ρ(V,Vobs)
(1-33)
Figure RE-FDA0003521220480000101
其中，k、kv2为斥力场正常量；ρ(q,qobs)表示畜牧清洗自动驾驶车与障碍物两点之间的欧氏距离；ρ(V,Vobs)表示畜牧清洗自动驾驶车和动态障碍物之间的相对速度；ρ0为常数，表示障碍物对畜牧清洗自动驾驶车产生作用的最大范围；α为障碍物的相对移动方向和移动畜牧清洗自动驾驶车与障碍物连线的夹角，当
Figure RE-FDA0003521220480000102
时，障碍物的移动方向确定为靠近移动畜牧清洗自动驾驶车；
(2)当障碍物和畜牧舍出口对移动畜牧清洗自动驾驶车产生的排斥力和吸引力相等时，增设虚拟子目标法，借助虚拟外力，使畜牧清洗自动驾驶车摆脱陷阱，成功抵达出口，首先依据畜牧清洗自动驾驶车的状态信息判断是否进入局部最小值陷阱，以连续五个步长作为判断依据，当总的移动距离小于βL(L为步长，β∈[2,4]时，改变β，提前检测到自身陷入局部极小值陷阱，其次程序跳转到虚拟子目标增设模块，存储使移动畜牧清洗自动驾驶车陷入局部最小值陷阱的障碍物位置信息，形成障碍物群，充分考虑出口位置与障碍物群位置信息，判断障碍物在移动畜牧清洗自动驾驶车与出口连接线左右两端数目，选择障碍物群左侧或右侧为目标障碍物，然后利用下式得到虚拟子目标位置：
Figure RE-FDA0003521220480000103
式中，xob,yob为目标障碍物位置；(xg,yg)为原畜牧舍出口所处位置；(x,y)为畜牧清洗自动驾驶车的当前位置；β1，β2为可调参数，β1，β2>0；移动畜牧清洗自动驾驶车摆脱上述陷阱后，将撤出虚拟子目标，移动畜牧清洗自动驾驶车不再受外力作用。移动畜牧清洗自动驾驶车将在原目标和障碍物的合力作用下向原畜牧舍出口靠近；
(3)当畜牧清洗自动驾驶车探测到的障碍物数目大于设定值或畜牧清洗自动驾驶车探测到障碍物与自身相对位置小于设定值时，环境即为复杂环境。在这种情况下，畜牧清洗自动驾驶车步长取Lmin；而当畜牧清洗自动驾驶车离目标很近时，为避免出现步长太大而越过过出口，或者步长太小而无法跟踪出口的情况，步长取Lmid；当畜牧清洗自动驾驶车处于简单环境时，步长取Lmax。规划出一条局部路径；
随后上位机通过串口将路径信息发送给主控制器，主控器将上位机发来的路径信息通过串口中断进行接收，打包完成后然后通过CAN总通信将畜牧清洗自动驾驶车的前进、后退、刹车、左转和右转对应的控制命令发送到总线上，然后设置一个CAN总线发送标志位，当一个模块的数据打包完成后将标志位flag置1，然后通过CAN总线发送该模块的动作信息给从控制器；
从控制器通过CAN总线方式与主控器相连，从控制器包括两部分设计，一是CAN总线接收软件设计，二是使用PWM方式对各个舵机的脉冲宽度进行控制，实现畜牧清洗车的前进、后退、刹车和转向；
从控制器CAN总线接收设计采用STM32F103ZET6自带的一个bx_CAN控制器、收发器是TJA1050芯片，从控制器上电后需要进行CAN总线部分的初始化，其中主要包括对从节点号、过滤器、波特率、中断的设置。从节点从CAN总线上接收数据，然后CAN控制器对所接收数据进行过滤，得到对应各控制的数据信息；
从控制器PWM舵机舵角控制设计采用的是STM32F103ZET6，它具有两个16位高级定时器TIM1、TIM8(带死区控制，有互补输出)和四个16位通用定时器TIM2～TIM5，均可输出PWM，产生DMA请求，以此完成畜牧清洗车的前进、后退、刹车和转向；
所述步骤十根据步骤八获取的畜牧舍目标三维检测模型实时获取清洗对象的三维信息，匹配步骤三所得畜牧舍清洗对象3D模型，获取挡板、栅栏的准确清洗范围，通过PLC控制摆动电机和往复电机实现对清洗喷头的摆动和清洗杆的旋转，完成对畜牧舍的栅栏、挡板的全方位清洗；
首先测试PLC串口是否连接成功，其次根据实时获取的挡板、栅栏的位置、大小、距离三维信息，匹配步骤三获取的畜牧舍清洗对象3D场景模型，以确定清洗目标的范围，若检测到栅栏时，通过PLC控制摆动电机控制清洗喷头纵向摆动，保证栅栏全方位清洗，若检测到挡板时，确定栅栏位置及大小形状信息，以锁定死角位置，随后通过PLC控制往复电机控制清洗杆上下摆动，达到死角反复清洗的目的，从而全方位清洗畜牧舍。
CN202111075136.5A 2021-09-14 2021-09-14 一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法 Pending CN114219910A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111075136.5A CN114219910A (zh) 2021-09-14 2021-09-14 一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111075136.5A CN114219910A (zh) 2021-09-14 2021-09-14 一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法

Publications (1)

Publication Number Publication Date
CN114219910A true CN114219910A (zh) 2022-03-22

Family

ID=80695927

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111075136.5A Pending CN114219910A (zh) 2021-09-14 2021-09-14 一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法

Country Status (1)

Country Link
CN (1) CN114219910A (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114820819A (zh) * 2022-05-26 2022-07-29 广东机电职业技术学院 快速路自动驾驶方法及系统
CN116448118A (zh) * 2023-04-17 2023-07-18 深圳市华辰信科电子有限公司 一种扫地机器人的工作路径优化方法和装置
CN116630394A (zh) * 2023-07-25 2023-08-22 山东中科先进技术有限公司 一种三维建模约束的多模态目标物体姿态估计方法及系统
CN116687386A (zh) * 2023-08-07 2023-09-05 青岛市畜牧工作站(青岛市畜牧兽医研究所) 牛体形数据综合校准的雷达探测系统及方法
CN117237902A (zh) * 2023-11-15 2023-12-15 山东飞宏工程机械有限公司 基于深度学习的机器人物体识别系统
CN117409213A (zh) * 2023-12-15 2024-01-16 北京南天智联信息科技股份有限公司 一种数据特征抽取方法和系统
CN117470249A (zh) * 2023-12-27 2024-01-30 湖南睿图智能科技有限公司 基于激光点云和视频图像融合感知的船舶防碰撞方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180339703A1 (en) * 2017-05-26 2018-11-29 Sucxess LLC Vehicle with remote-controlled operating mode
CN109443369A (zh) * 2018-08-20 2019-03-08 北京主线科技有限公司 利用激光雷达和视觉传感器融合构建动静态栅格地图的方法
CN111554088A (zh) * 2020-04-13 2020-08-18 重庆邮电大学 一种多功能v2x智能路侧基站系统
CN112894758A (zh) * 2021-04-01 2021-06-04 广东南牧机械设备有限公司 机器人清洗控制系统、方法、装置和计算机设备

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180339703A1 (en) * 2017-05-26 2018-11-29 Sucxess LLC Vehicle with remote-controlled operating mode
CN109443369A (zh) * 2018-08-20 2019-03-08 北京主线科技有限公司 利用激光雷达和视觉传感器融合构建动静态栅格地图的方法
CN111554088A (zh) * 2020-04-13 2020-08-18 重庆邮电大学 一种多功能v2x智能路侧基站系统
CN112894758A (zh) * 2021-04-01 2021-06-04 广东南牧机械设备有限公司 机器人清洗控制系统、方法、装置和计算机设备

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114820819B (zh) * 2022-05-26 2023-03-31 广东机电职业技术学院 快速路自动驾驶方法及系统
CN114820819A (zh) * 2022-05-26 2022-07-29 广东机电职业技术学院 快速路自动驾驶方法及系统
CN116448118B (zh) * 2023-04-17 2023-10-31 深圳市华辰信科电子有限公司 一种扫地机器人的工作路径优化方法和装置
CN116448118A (zh) * 2023-04-17 2023-07-18 深圳市华辰信科电子有限公司 一种扫地机器人的工作路径优化方法和装置
CN116630394A (zh) * 2023-07-25 2023-08-22 山东中科先进技术有限公司 一种三维建模约束的多模态目标物体姿态估计方法及系统
CN116630394B (zh) * 2023-07-25 2023-10-20 山东中科先进技术有限公司 一种三维建模约束的多模态目标物体姿态估计方法及系统
CN116687386A (zh) * 2023-08-07 2023-09-05 青岛市畜牧工作站(青岛市畜牧兽医研究所) 牛体形数据综合校准的雷达探测系统及方法
CN116687386B (zh) * 2023-08-07 2023-10-31 青岛市畜牧工作站(青岛市畜牧兽医研究所) 牛体形数据综合校准的雷达探测系统及方法
CN117237902A (zh) * 2023-11-15 2023-12-15 山东飞宏工程机械有限公司 基于深度学习的机器人物体识别系统
CN117237902B (zh) * 2023-11-15 2024-01-26 山东飞宏工程机械有限公司 基于深度学习的机器人物体识别系统
CN117409213A (zh) * 2023-12-15 2024-01-16 北京南天智联信息科技股份有限公司 一种数据特征抽取方法和系统
CN117470249A (zh) * 2023-12-27 2024-01-30 湖南睿图智能科技有限公司 基于激光点云和视频图像融合感知的船舶防碰撞方法及系统
CN117470249B (zh) * 2023-12-27 2024-04-02 湖南睿图智能科技有限公司 基于激光点云和视频图像融合感知的船舶防碰撞方法及系统

Similar Documents

Publication Publication Date Title
CN114219910A (zh) 一种融合激光雷达和机器视觉的自动驾驶车畜牧清洗方法
CN106599108B (zh) 一种三维环境中多模态环境地图构建方法
CN113345018B (zh) 一种动态场景下的激光单目视觉融合定位建图方法
US9305219B2 (en) Method for estimating free space using a camera system
Zhang et al. Instance segmentation of lidar point clouds
CN104732587B (zh) 一种基于深度传感器的室内3d语义地图构建方法
Murthy et al. Shape priors for real-time monocular object localization in dynamic environments
CN109238288A (zh) 一种无人机室内自主导航方法
CN111679661A (zh) 基于深度相机的语义地图构建方法及扫地机器人
CN113345008A (zh) 一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法
Liu et al. Real-time 6d lidar slam in large scale natural terrains for ugv
CN114454875A (zh) 一种基于强化学习的城市道路自动泊车方法及系统
Choe et al. Fast point cloud segmentation for an intelligent vehicle using sweeping 2D laser scanners
Hardouin et al. Surface-driven Next-Best-View planning for exploration of large-scale 3D environments
CN116109601A (zh) 一种基于三维激光雷达点云的实时目标检测方法
Menon et al. NBV-SC: Next best view planning based on shape completion for fruit mapping and reconstruction
Protasov et al. Cnn-based omnidirectional object detection for hermesbot autonomous delivery robot with preliminary frame classification
CN115294287A (zh) 一种用于温室巡检机器人的激光slam建图方法
Xiong et al. Road-Model-Based road boundary extraction for high definition map via LIDAR
Short et al. Abio-inspiredalgorithminimage-based pathplanning and localization using visual features and maps
Vatavu et al. Modeling and tracking of dynamic obstacles for logistic plants using omnidirectional stereo vision
Chen et al. Monocular semantic SLAM in dynamic street scene based on multiple object tracking
Wang et al. DRR-LIO: A dynamic-region-removal-based LiDAR inertial odometry in dynamic environments
Zhou et al. Fine-level semantic labeling of large-scale 3d model by active learning
Zhu et al. Road curb detection using a novel tensor voting algorithm

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination