CN114637302B - 一种基于计算机视觉的自动行进避障方法及系统 - Google Patents

一种基于计算机视觉的自动行进避障方法及系统 Download PDF

Info

Publication number
CN114637302B
CN114637302B CN202210398942.4A CN202210398942A CN114637302B CN 114637302 B CN114637302 B CN 114637302B CN 202210398942 A CN202210398942 A CN 202210398942A CN 114637302 B CN114637302 B CN 114637302B
Authority
CN
China
Prior art keywords
traveling
camera
obstacle
equipment
obstacle avoidance
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.)
Active
Application number
CN202210398942.4A
Other languages
English (en)
Other versions
CN114637302A (zh
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.)
Anhui Agricultural University AHAU
Original Assignee
Anhui Agricultural University AHAU
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 Anhui Agricultural University AHAU filed Critical Anhui Agricultural University AHAU
Priority to CN202210398942.4A priority Critical patent/CN114637302B/zh
Publication of CN114637302A publication Critical patent/CN114637302A/zh
Application granted granted Critical
Publication of CN114637302B publication Critical patent/CN114637302B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Abstract

本发明涉及数字化农业机械技术领域，公开了一种基于计算机视觉的自动行进避障方法及系统，方法包括如下步骤：步骤S100：在行进设备上设置相机的拍摄角度和激光雷达的位置和探测角度；步骤S200：初始化相机和激光雷达；步骤S300：联合标定相机和激光雷达的坐标系；步骤S400：按照预设测距规则计算行进设备当前的行进速度和位置信息；步骤S500：判断行进方向上是否存在障碍物；若存在，进入保步骤S600；若不存在，则进入步骤S700；步骤S600：按照预设避障规则进行行进规划路径的变更；步骤S700：按照行进规划路径控制行进方向和行进速度。从而能够按照行进规划路径及时的控制行进方向和行进速度保证行进过程中高效和稳定的避障。

Description

一种基于计算机视觉的自动行进避障方法及系统
技术领域
本发明涉及数字化农业机械技术领域，具体涉及一种基于计算机视觉的自动行进避障方法及系统。
背景技术
随着农业机械技术的发展，农业机械向自动化、智能化方向的发展成为当今农业研究的热点。一种成本低廉，使用简单的农业机械全自动导航系统，在农业生产中有着重要作用。它可以降低成本，提高效率，带来更多经济效益。
农业机械在田地里自动运作需要进行避障，传统避障方法使用最短切线法，采用这种方法生成快速简单、轨迹短，但是最短路径曲线的曲率不连续并且在直线与圆弧连接处存在弯折拐点。会导致在刚开始转弯和经过障碍，恢复直线这两次行驶工况，前轮转角骤变，导致机械运行不稳定；而且在避障时所采用的相机和测距传感器由于安装位置不同，容易导致避障产生偏差，另外现有的避障方法会不加区分的进行避障，无论遇到何种障碍，都会进行避障，因此非常影响农业机械的避障行驶精度和降低避障过程的成功率。
发明内容
本发明的目的在于提供一种基于计算机视觉的自动行进避障方法及系统，解决以下技术问题：
如何提供一种能够更加高效且可靠的用于自动行进避障的方法。
本发明的目的可以通过以下技术方案实现：
一种基于计算机视觉的自动行进避障方法，包括如下步骤：
步骤S100：在行进设备上设置相机的拍摄角度和激光雷达的位置和探测角度；
步骤S200：初始化所述相机和所述激光雷达；
步骤S300：联合标定所述相机和所述激光雷达的坐标系；
步骤S400：按照预设测距规则计算所述行进设备当前的行进速度和位置信息；
步骤S500：判断行进方向上是否存在障碍物；若存在，进入步骤S600；若不存在，则进入步骤S700；
步骤S600：按照预设避障规则进行行进规划路径的变更；
步骤S700：按照行进规划路径控制行进方向和行进速度。
通过上述技术方案，通过预先设置相机和激光雷达的各项参数以及相互位置关系，可以对行进设备的行进方向上的各类阻碍进行确定，之后可对阻碍的类型进行判断，一旦确定发现障碍物并确定其位置，便可自动的按照预设避障规则进行行进规划路径的变更，从而能够按照行进规划路径及时的控制行进方向和行进速度保证行进过程中高效和稳定的避障。
作为本发明进一步的方案：所述步骤S300包括：
步骤S301：标定所述相机的内参和外参；
步骤S302：确定所述相机与所述激光雷达的坐标系关系；
步骤S303：确定所述相机与图像像素坐标系的关系；
步骤S304：确定所述激光雷达与所述图像像素的坐标系的关系；
其中，所述图像像素的坐标系表示所述相机获取的图像中像素点在所述图像中的位置。
通过上述技术方案，通过上述的若干次变换可将激光雷达所发现的目标的位置直接变换到相机获取的图像中，从而能够有助于后续对障碍物的确定以及定位，提高障碍物的定位精准度和效率。
作为本发明进一步的方案：所述步骤S400包括：
步骤S401：获取行进方向范围内距离所述行进设备最近的采样点和测距点的位置；
步骤S402：根据所述测距点与所述采样点以及所述行进设备的相对位置变化量计算所述行进速度和所述位置信息；
其中，所述测距点距离所述行进设备的距离小于所述采样点与所述行进设备的距离。
通过上述技术方案，通过取距离行进设备的行进方向左右的最近两处可能为障碍物的物体分别作为采样点和测距点，取距离行进设备更近的一处为测距点，可通过测距点与行进设备的距离以及测距点、采样点与行进设备之间夹角随时间变化的情况来对行进速度和位置信息进行计算。
作为本发明进一步的方案：所述步骤S402包括：
通过所述激光雷达的点云数据获取所述行进速度；
所述行进速度为所述行进设备底盘中心线的线速度v，所述行进速度的计算公式如下：
Figure GDA0003816386270000031
其中，Δd为在Δt时间内所述行进设备与所述测距点的距离变化量；d0为Δt时间之前所述行进设备与测距点之间距离；α为Δt时间之前所述行进方向与所述行进设备和所述测距点之间连线的夹角；d1为所述行进设备当前与所述测距点之间距离；β为当前行进方向与所述行进设备和所述测距点之间连线的夹角；tn为激光雷达设置为n赫兹时单位时间变化量，取值为
Figure GDA0003816386270000041
作为本发明进一步的方案：所述步骤S500包括：
步骤S501：通过所述相机获取所述行进方向的采样图片；
步骤S502：将所述采样图片发送识别模块，由所述识别模块对所述采样图片中的物体进行识别，并输出判断结果；所述判断结果包括障碍物和其他物；
若所述判断结果为障碍物，则进入步骤S600；
若所述判断结果为其他物，则进入步骤S700；
其中，所述识别模块为训练完成的神经网络模型。
通过上述技术方案，可以识别障碍物是否会影响行进设备继续行驶，若是属于小型障碍物则可以按照其他物进行分类，如果是农作物或者大型障碍物需要认定为障碍物，进行避障绕行。比如，前方为小块的草堆障碍物，行进设备可以安全通过，也不会对农作物产生碾压，因此可以通过机器视觉中训练完成的神经网络模型识别该物体是不是必须避开的障碍物，如此可以减少行进设备机动的频率，提升工作效率。
作为本发明进一步的方案：所述步骤S600中的所述预设避障规则包括：
步骤S601：获取所述障碍物与所述行进设备的距离一l1和距离二l2；
步骤S602：根据所述障碍物的形态半径r为半径生成障碍物特征圆；
其中，
Figure GDA0003816386270000042
l1为所述行进设备的一边与所述障碍物的距离；l2为所述行进设备的另一边与所述障碍物的距离；γ是所述行进设备的两边分别与所述障碍物之间的两条连线之间所形成的夹角。
通过上述技术方案，可以把障碍物作为以障碍物特征圆的半径的圆柱体看待，有利于简化行进规划路径，避免行进规划路径过于复杂。
作为本发明进一步的方案：所述预设避障规则还包括：
步骤S603：以障碍物特征圆作为避障对象进行所述行进规划路径的变更；
其中，所述行进规划路径包括：使用两段对称的三阶Bezier曲线绘制导航标定线，作为所述行进设备路径规划时导航路径的拟合路径。
通过上述技术方案，使得行进规划路径可以分为对称的接近段和离去段，三阶Bezier曲线做行进规划路径比切线圆弧更平滑，与行进设备的特性契合。
作为本发明进一步的方案：所述步骤S603包括：
步骤S5031：选取n+1个控制点Pi；
步骤S5032：根据所述控制点Pi获得n阶Bezier曲线；
步骤S5033：通过对所述n阶Bezier曲线求导获取曲率公式；
其中，0≤i≤n，n＝3，Pi的坐标为(xi，yi)，所述n阶Bezier曲线为所述导航标定线。
通过上述技术方案，可通过选取的4个控制点获得三阶Bezier曲线，再对该三阶Bezier曲线进行求导即可得到该行进规划路径各个位置的曲率，从而方便对行进设备的转向轮进行定量控制。
作为本发明进一步的方案：所述步骤S600包括：
步骤S601：通过所述导航标定线的角度和方向控制所述行进设备的电机控制器；
步骤S602：通过控制所述行进设备的前轮转向角和电机转速来控制所述行进设备的航向角和行进速度；
其中，所述前轮转向角和所述行进设备的转向之间的关系式为：
Figure GDA0003816386270000061
Figure GDA0003816386270000062
Figure GDA0003816386270000063
Figure GDA0003816386270000064
其中，α为所述前轮转向角，
Figure GDA0003816386270000065
为所述行进设备的航向角，R为所述行进设备的转弯半径，L为所述行进设备的前后轮轴距。
通过上述技术方案，可通过对行进设备的前轮转向角和电机转速的控制来控制行进设备的行进方向和行进速度，保证避障工作的顺利执行。
一种基于计算机视觉的自动行进避障系统，包括安装在行进设备上的相机、激光雷达、轮胎转角传感器和总控制器；
所述相机和所述激光雷达安装在所述行进设备的前端，所述轮胎转角传感器安装在所述行进设备的轮胎支撑轴上；
所述相机和所述激光雷达以及所述轮胎转角传感器均与所述总控制器连接。
本发明的有益效果：
(1)本发明通过预先设置相机和激光雷达的各项参数以及相互位置关系，可以精准的对行进设备的行进方向上的各类阻碍进行确定，之后可对阻碍的类型进行判断，一旦确定发现障碍物并确定其位置，便可自动的按照预设避障规则进行行进规划路径的变更，从而能够按照行进规划路径及时的控制行进方向和行进速度保证行进过程中高效和稳定的避障；
(2)本发明的行进规划路径采用对称两条三阶Bezier曲线，使避障路径的曲率连续平滑无拐点，提高了避障的稳定性和成功率，提高系统鲁棒性，而且使用相对于传统技术采用更少的传感器以减少成本，适合大面积推广使用。
附图说明
下面结合附图对本发明作进一步的说明。
图1为本发明中自动行进避障方法的流程示意图；
图2为本发明中传感器联合标定原理示意图；
图3为本发明中激光雷达测距原理示意图；
图4为本发明中障碍物特征圆获取原理图；
图5为本发明中行进规划路径的变更原理示意图；
图6为本发明中行进设备的基本结构示意图。
附图说明：1、相机；2、激光雷达；3、轮胎转角传感器；4、总控制器。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
请参阅图1所示，本发明为一种基于计算机视觉的自动行进避障方法，包括如下步骤：
步骤S100：在行进设备上设置相机1的拍摄角度和激光雷达2的位置和探测角度；
步骤S200：初始化相机1和激光雷达2；
步骤S300：联合标定相机1和激光雷达2的坐标系；
步骤S400：按照预设测距规则计算行进设备当前的行进速度和位置信息；
步骤S500：判断行进方向上是否存在障碍物；若存在，进入步骤S600；若不存在，则进入步骤S700；
步骤S600：按照预设避障规则进行行进规划路径的变更；
步骤S700：按照行进规划路径控制行进方向和行进速度。
通过上述技术方案，通过预先设置相机1和激光雷达2的各项参数以及相互位置关系，可以对行进设备的行进方向上的各类阻碍进行确定，之后可对阻碍的类型进行判断，一旦确定发现障碍物并确定其位置，便可自动的按照预设避障规则进行行进规划路径的变更，从而能够按照行进规划路径及时的控制行进方向和行进速度保证行进过程中高效和稳定的避障。
作为本发明进一步的方案：步骤S300包括：
步骤S301：标定相机1的内参和外参；
步骤S302：确定相机1与激光雷达2的坐标系关系；
步骤S303：确定相机1与图像像素坐标系的关系；
步骤S304：确定激光雷达2与图像像素的坐标系的关系；
其中，图像像素坐标系表示相机1获取的图像中像素点在图片中的位置。
具体的，内参是相机1内摄像头本身的物理参数，受镜头和CMOS物理特性影响。
在步骤S302中，激光雷达2的坐标系与相机1的坐标系之间的位置关系可以分解为绕坐标原点OC的旋转和平移，计算公式如下：
Figure GDA0003816386270000091
R＝RxRyRz；
T＝[tx ty tz]T (1)；
Figure GDA0003816386270000092
Figure GDA0003816386270000093
Figure GDA0003816386270000094
其中，R为旋转矩阵，代表激光雷达2的坐标系与相机1的坐标系之间的角位移关系；α、β和γ分别为激光雷达2的坐标系在相机1的坐标系中沿XC、YC和ZC轴方向的偏转角度；T为平移矩阵，表示激光雷达2的坐标系中原点在相机1的坐标系中的坐标，与激光雷达2的坐标系与相机1的坐标系之间的相对位置关系有关。
在步骤S303中，从相机1的坐标系到图像像素的坐标系，是从三维空间坐标系到二维平面坐标系的转换。相机1将物点P成像到像素平面上的P′点，根据三角形相似原理，P′点的物理坐标(X′C，YC′)与P点(XC，YC，YC)的关系公式如下：
Figure GDA0003816386270000095
上式可以用矩阵表示为：
Figure GDA0003816386270000101
P′点的图像物理坐标(XC′，YC′)与像素坐标(u，v)的转换关系如下公式所示：
Figure GDA0003816386270000102
上式可以用矩阵表示为：
Figure GDA0003816386270000103
其中，如图2所示，相机1的内参数分别为：1/dx、1/dy、r、u0、v0、f。其中，dx、dy分别为摄像机的单个像素在XC′和YC′方向上的物理尺寸；r是倾斜因子，在一般标准相机1中为0；(u0，v0)是图像主点坐标，即摄像机光轴与物理成像平面相交点OC′的像素坐标。
由式(2)和式(3)可得出相机1的坐标系与图像像素的坐标系之间的转换关系：
Figure GDA0003816386270000104
其中，
Figure GDA0003816386270000105
和
Figure GDA0003816386270000106
分别表示XC′轴和YC′轴方向上的等效焦距。
在步骤S303中，由式(1)和式(4)可以得到激光雷达2的坐标系与图像像素的坐标系之间的转换关系为：
Figure GDA0003816386270000111
通过上述的坐标变换转化可将激光雷达2所发现的目标的位置与相机1获取图像中的指定位置像素进行关联对应，从而能够有助于后续对障碍物的确定以及定位，提高障碍物的定位精准度和效率。
作为本发明进一步的方案：步骤S400包括：
步骤S401：获取行进方向范围内距离行进设备最近的采样点和测距点的位置；
步骤S402：根据测距点与采样点以及行进设备的相对位置变化量计算行进速度和位置信息；
其中，测距点距离行进设备的距离小于采样点与行进设备的距离。
通过上述技术方案，通过取距离行进设备的行进方向左右的最近两处可能为障碍物的物体分别作为采样点和测距点，取距离行进设备更近的一处为测距点，可通过测距点与行进设备的距离以及测距点、采样点与行进设备之间夹角随时间变化的情况来对行进速度和位置信息进行计算。
作为本发明进一步的方案：步骤S402包括：
通过激光雷达2的点云数据获取行进速度；
行进速度为行进设备底盘中心线的线速度v，行进速度的计算公式如下：
Figure GDA0003816386270000112
其中，如图3所示，Δd为在Δt时间内行进设备与测距点的距离变化量；d0为Δt时间之前行进设备与测距点之间距离；α为Δt时间之前行进方向与行进设备和测距点之间连线的夹角；d1为行进设备当前与测距点之间距离；β为当前行进方向与行进设备和测距点之间连线的夹角；tn为激光雷达2设置为n赫兹时单位时间变化量，本实施例中可设置为30赫兹，t30取值为
Figure GDA0003816386270000121
作为本发明进一步的方案：步骤S500包括：
步骤S501：通过相机1获取行进方向的采样图片；
步骤S502：将采样图片发送识别模块，由识别模块对采样图片中的物体进行识别，并输出判断结果；判断结果包括障碍物和其他物；
若判断结果为障碍物，则进入步骤S600；
若判断结果为其他物，则进入步骤S700；
其中，识别模块为训练完成的神经网络模型。
通过上述技术方案，可以识别障碍物是否会影响行进设备继续行驶，若是属于小型障碍物则可以按照其他物进行分类，如果是农作物或者大型障碍物需要认定为障碍物，进行避障绕行。
比如，前方为小块的草堆障碍物，行进设备可以安全通过，也不会对农作物产生碾压，因此可以通过机器视觉中训练完成的神经网络模型识别该物体是不是必须避开的障碍物，如此可以减少行进设备机动的频率，提升工作效率。
另外，在本实施例中，可以将激光雷达2的探测信号输入识别模块，该探测信号帮助相机识别采样图片中的物体距离和体积，例如，识别模块判定采样图片中的物体类型为土块，而非农作物，此时可以利用之前相机1和激光雷达2的标定步骤，确定该土块的距离和体积，从而可以继续判断该土块是大土块还是小土块，若是大土块则需要进行避障，若是小土块则不需要，进一步减少行进设备机动的频率，提升工作效率。
在识别模块进行训练时，可采集不同环境下的农作物以及农田常见障碍物图像，采用随机梯度下降法对网络进行训练，训练过程中，当验证集的分类精度停止增加的时候，则降低学习率，降低的方式是将学习速率变为当前速率的1/10，直至通过调整学习速率不再提高识别率为止，即可完成对识别模块的训练。
作为本发明进一步的方案：步骤S600中的预设避障规则包括：
步骤S601：获取障碍物与行进设备的距离一l1和距离二l2；
步骤S602：根据障碍物的形态半径r为半径生成障碍物特征圆；
其中，如图4和图5所示，
Figure GDA0003816386270000131
l1为行进设备的一边与障碍物的距离；l2为行进设备的另一边与障碍物的距离；γ是行进设备的两边分别与障碍物之间的两条连线之间所形成的夹角。
通过上述技术方案，可以把障碍物作为以障碍物特征圆的半径的圆柱体看待，有利于简化行进规划路径，避免行进规划路径过于复杂。
作为本发明进一步的方案：预设避障规则还包括：
步骤S603：以障碍物特征圆作为避障对象进行行进规划路径的变更；
其中，行进规划路径包括：使用两段对称的三阶Bezier曲线绘制导航标定线，作为行进设备路径规划时导航路径的拟合路径。
通过上述技术方案，使得行进规划路径可以分为对称的接近段和离去段，三阶Bezier曲线做行进规划路径比切线圆弧更平滑，与行进设备的特性契合。
作为本发明进一步的方案：步骤S603包括：
步骤S5031：选取n+1个控制点Pi；
步骤S5032：根据控制点Pi获得n阶Bezier曲线；
步骤S5033：通过对n阶Bezier曲线求导获取曲率公式；
其中，0≤i≤n，n＝3，Pi的坐标为(xi，yi)，该三阶Bezier曲线为导航标定线。
在步骤S5031中，三阶Bezier曲线需要由四个控制点P0、P1、P2、P3在平面或在三维空间中来定义。如图5所示，三阶Bezier曲线从P0点开始向P1方向移动，但是通常不会经过P1，之后向P2方向移动，但也不会经过P2，最后到达P3点，采用P1和P2只用来控制方向走向，顺次连接从P0到Pn的折线被称为Bezier曲线的控制多边形。
在步骤S5032中，该n阶Bezier曲线可以表示为：
Figure GDA0003816386270000141
其中，Bi，n(u)为n次Bernstein多项式，其表达式为：
Figure GDA0003816386270000142
上述三阶Bezier曲线矩阵表示为：
Figure GDA0003816386270000143
可得：
Figure GDA0003816386270000151
在坐标系XOY中，对于给定已知的四个控制点P0(x0，y0)，P1(x1，y1)，P2(x2，y2)，P3(x3，y3)可以由下列公式(5)和公式(6)确定一个三阶Bezier曲线，公式(5)和公式(6)可以获得控制点的横纵轴坐标。
Figure GDA0003816386270000152
Figure GDA0003816386270000153
在步骤S5033中，对三阶Bezier曲线进行求导，可得公式：
Figure GDA0003816386270000154
该三阶Bezier曲线上任意一点的曲率公式为：
Figure GDA0003816386270000155
由以上公式可知，当且仅当x′(μ)＝y′(μ)＝0时曲率不连续。但是在这种情况下，Bezier曲线将会缩成一个点，所以三阶Bezier曲线处处连续。
若需要加上曲率约束，来满足行进设备的约束条件。可将行进设备的转弯半径表示为R，可以得到：
最大曲率：
Figure GDA0003816386270000156
最小曲率：
Kmin＝0；
所以，曲率上下有界约束条件可以表示为：
Kmin≤K(μ)≤Kmax。
通过上述技术方案，可通过选取的4个控制点获得三阶Bezier曲线，再对该三阶Bezier曲线进行求导即可得到该行进规划路径各个位置的曲率，从而方便对行进设备的转向轮进行定量控制。
作为本发明进一步的方案：步骤S600包括：
步骤S601：通过导航标定线的角度和方向控制行进设备的电机控制器；
步骤S602：通过控制行进设备的前轮转向角和电机转速来控制行进设备的航向角和行进速度；
其中，前轮转向角和行进设备的转向之间的关系式为：
Figure GDA0003816386270000161
Figure GDA0003816386270000162
Figure GDA0003816386270000163
Figure GDA0003816386270000164
其中，α为前轮转向角，
Figure GDA0003816386270000165
为行进设备的航向角，R为行进设备的转弯半径，L为行进设备的前后轮轴距。
通过上述技术方案，可通过dwa算法计算行进速度，通过对行进设备的前轮转向角和电机转速的控制来控制行进设备的行进方向和行进速度，保证避障工作的顺利执行。
一种基于计算机视觉的自动行进避障系统，如图6所示，包括安装在行进设备上的相机1、激光雷达2、轮胎转角传感器3和总控制器4；
相机1和激光雷达2安装在行进设备的前端，轮胎转角传感器3安装在行进设备的轮胎支撑轴上；
相机1和激光雷达2以及轮胎转角传感器3均与总控制器4连接。其中，相机1通过USB接口、激光雷达2通过网口与总控制器4连接，轮胎转角传感器3通过RS-485接口与总控制器4相连。
本发明的工作原理：本发明利用计算机视觉以及神经网络模型的深度学习算法通过相机1识别障碍物以及农作物种类，再利用激光雷达2进行测距、测速从而判断行进设备自身相对农作物的位置，并将激光雷达2和相机1数据发送给总控制器4，总控制器4对数据进行融合转换分析，在作物行间规划出行进规划路径，再根据当前状态调整轮胎的转速以调整行进设备的运动状态，根据导航标定线的角度和方向控制行进设备的轮胎转速控制器，通过控制行进设备的前轮转角来控制行进设备的移动和转向。行进规划路径采用对称两条三阶Bezier曲线，使避障路径曲率连续平滑无拐点，提高了避障的稳定性和成功率，此外使用相对于传统技术更少的传感器以减少成本，提高系统鲁棒性。
以上对本发明的一个实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。

Claims (8)

1.一种基于计算机视觉的自动行进避障方法，其特征在于，包括如下步骤：
步骤S100：在行进设备上设置相机(1)的拍摄角度和激光雷达(2)的位置和探测角度；
步骤S200：初始化所述相机(1)和所述激光雷达(2)；
步骤S300：联合标定所述相机(1)和所述激光雷达(2)的坐标系；
步骤S400：按照预设测距规则计算所述行进设备当前的行进速度和位置信息；
步骤S500：判断行进方向上是否存在障碍物；若存在，进入步骤S600；若不存在，则进入步骤S700；
步骤S600：按照预设避障规则进行行进规划路径的变更；
步骤S700：按照行进规划路径控制行进方向和行进速度；
所述步骤S400包括：
步骤S401：获取行进方向范围内距离所述行进设备最近的采样点和测距点的位置；
步骤S402：根据所述测距点与所述采样点以及所述行进设备的相对位置变化量计算所述行进速度和所述位置信息；
其中，所述测距点距离所述行进设备的距离小于所述采样点与所述行进设备的距离；
所述步骤S402包括：
通过所述激光雷达(2)的点云数据获取所述行进速度；
所述行进速度为所述行进设备底盘中心线的线速度v，所述行进速度的计算公式如下：
Figure FDA0003837178160000011
其中，Δd为在Δt时间内所述行进设备与所述测距点的距离变化量；d0为Δt时间之前所述行进设备与测距点之间距离；α为Δt时间之前所述行进方向与所述行进设备和所述测距点之间连线的夹角；d1为所述行进设备当前与所述测距点之间距离；β为当前行进方向与所述行进设备和所述测距点之间连线的夹角；tn为激光雷达(2)设置为n赫兹时单位时间变化量，取值为
Figure FDA0003837178160000021
2.根据权利要求1所述的基于计算机视觉的自动行进避障方法，其特征在于，所述步骤S300包括：
步骤S301：标定所述相机(1)的内参和外参；
步骤S302：确定所述相机(1)与所述激光雷达(2)的坐标系关系；
步骤S303：确定所述相机(1)与图像像素坐标系的关系；
步骤S304：确定所述激光雷达(2)与所述图像像素的坐标系的关系；
其中，所述图像像素的坐标系表示所述相机(1)获取的图像中像素点在所述图像中的位置。
3.根据权利要求1所述的基于计算机视觉的自动行进避障方法，其特征在于，所述步骤S500包括：
步骤S501：通过所述相机(1)获取所述行进方向的采样图片；
步骤S502：将所述采样图片发送识别模块，由所述识别模块对所述采样图片中的物体进行识别，并输出判断结果；所述判断结果包括障碍物和其他物；
若所述判断结果为障碍物，则进入步骤S600；
若所述判断结果为其他物，则进入步骤S700；
其中，所述识别模块为训练完成的神经网络模型。
4.根据权利要求1所述的基于计算机视觉的自动行进避障方法，其特征在于，所述步骤S600中的所述预设避障规则包括：
步骤S601：获取所述障碍物与所述行进设备的距离一l1和距离二l2；
步骤S602：根据所述障碍物的形态半径r为半径生成障碍物特征圆；
其中，
Figure FDA0003837178160000031
l1为所述行进设备的一边与所述障碍物的距离；l2为所述行进设备的另一边与所述障碍物的距离；γ是所述行进设备的两边分别与所述障碍物之间的两条连线之间所形成的夹角。
5.根据权利要求4所述的基于计算机视觉的自动行进避障方法，其特征在于，所述预设避障规则还包括：
步骤S603：以障碍物特征圆作为避障对象进行所述行进规划路径的变更；
其中，所述行进规划路径包括：使用两段对称的三阶Bezier曲线绘制导航标定线，作为所述行进设备路径规划时导航路径的拟合路径。
6.根据权利要求5所述的基于计算机视觉的自动行进避障方法，其特征在于，所述步骤S603包括：
步骤S5031：选取n+1个控制点Pi；
步骤S5032：根据所述控制点Pi获得n阶Bezier曲线；
步骤S5033：通过对所述n阶Bezier曲线求导获取曲率公式；
其中，0≤i≤n，n＝3，Pi的坐标为(xi，yi)，所述n阶Bezier曲线为所述导航标定线。
7.根据权利要求6所述的基于计算机视觉的自动行进避障方法，其特征在于，所述步骤S600包括：
步骤S601：通过所述导航标定线的角度和方向控制所述行进设备的电机控制器；
步骤S602：通过控制所述行进设备的前轮转向角和电机转速来控制所述行进设备的航向角和行进速度；
其中，所述前轮转向角和所述行进设备的转向之间的关系式为：
Figure FDA0003837178160000041
Figure FDA0003837178160000042
Figure FDA0003837178160000043
Figure FDA0003837178160000044
其中，α为所述前轮转向角，
Figure FDA0003837178160000045
为所述行进设备的航向角，R为所述行进设备的转弯半径，L为所述行进设备的前后轮轴距。
8.一种采用权利要求1-7中任意一项所述方法的基于计算机视觉的自动行进避障系统，其特征在于，包括安装在行进设备上的相机(1)、激光雷达(2)、轮胎转角传感器(3)和总控制器(4)；
所述相机(1)和所述激光雷达(2)安装在所述行进设备的前端，所述轮胎转角传感器(3)安装在所述行进设备的轮胎支撑轴上；
所述相机(1)和所述激光雷达(2)以及所述轮胎转角传感器(3)均与所述总控制器(4)连接。
CN202210398942.4A 2022-04-15 2022-04-15 一种基于计算机视觉的自动行进避障方法及系统 Active CN114637302B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210398942.4A CN114637302B (zh) 2022-04-15 2022-04-15 一种基于计算机视觉的自动行进避障方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210398942.4A CN114637302B (zh) 2022-04-15 2022-04-15 一种基于计算机视觉的自动行进避障方法及系统

Publications (2)

Publication Number Publication Date
CN114637302A CN114637302A (zh) 2022-06-17
CN114637302B true CN114637302B (zh) 2022-10-18

Family

ID=81951234

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210398942.4A Active CN114637302B (zh) 2022-04-15 2022-04-15 一种基于计算机视觉的自动行进避障方法及系统

Country Status (1)

Country Link
CN (1) CN114637302B (zh)

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
JP2016057660A (ja) * 2014-09-05 2016-04-21 株式会社Ihi 移動体の経路計画方法と装置
CN206421229U (zh) * 2017-01-22 2017-08-18 无锡卡尔曼导航技术有限公司 一种基于北斗的农机自动驾驶控制装置
CN108663681A (zh) * 2018-05-16 2018-10-16 华南理工大学 基于双目摄像头与二维激光雷达的移动机器人导航方法
CN109491796A (zh) * 2018-11-23 2019-03-19 西安电子科技大学 基于累积和并行化的机载雷达运动速度估计方法
CN212294476U (zh) * 2019-12-31 2021-01-05 天嘉智能装备制造江苏股份有限公司 无人驾驶小型扫路车
CN112269965A (zh) * 2020-08-10 2021-01-26 中国北方车辆研究所 一种非完整约束条件下的连续曲率路径优化方法
CN112462766A (zh) * 2020-11-23 2021-03-09 江苏科技大学 一种用于无人船自主避障的装置及方法
CN113460066A (zh) * 2021-08-11 2021-10-01 东南大学 一种检测自动驾驶车辆队列中车辆数据传输准确性的方法
WO2021208225A1 (zh) * 2020-04-15 2021-10-21 长沙中联重科环境产业有限公司 一种防疫消毒清洁机器人避障方法、装置及设备
CN113759900A (zh) * 2021-08-12 2021-12-07 中南大学 基于障碍区域预测的巡检机器人轨迹规划与实时避障方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11768504B2 (en) * 2020-06-10 2023-09-26 AI Incorporated Light weight and real time slam for robots

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2016057660A (ja) * 2014-09-05 2016-04-21 株式会社Ihi 移動体の経路計画方法と装置
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
CN206421229U (zh) * 2017-01-22 2017-08-18 无锡卡尔曼导航技术有限公司 一种基于北斗的农机自动驾驶控制装置
CN108663681A (zh) * 2018-05-16 2018-10-16 华南理工大学 基于双目摄像头与二维激光雷达的移动机器人导航方法
CN109491796A (zh) * 2018-11-23 2019-03-19 西安电子科技大学 基于累积和并行化的机载雷达运动速度估计方法
CN212294476U (zh) * 2019-12-31 2021-01-05 天嘉智能装备制造江苏股份有限公司 无人驾驶小型扫路车
WO2021208225A1 (zh) * 2020-04-15 2021-10-21 长沙中联重科环境产业有限公司 一种防疫消毒清洁机器人避障方法、装置及设备
CN112269965A (zh) * 2020-08-10 2021-01-26 中国北方车辆研究所 一种非完整约束条件下的连续曲率路径优化方法
CN112462766A (zh) * 2020-11-23 2021-03-09 江苏科技大学 一种用于无人船自主避障的装置及方法
CN113460066A (zh) * 2021-08-11 2021-10-01 东南大学 一种检测自动驾驶车辆队列中车辆数据传输准确性的方法
CN113759900A (zh) * 2021-08-12 2021-12-07 中南大学 基于障碍区域预测的巡检机器人轨迹规划与实时避障方法及系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
An embedded straight navigation system for inter-row tree tracking implementation in agricultural field;Norashikin M. Thamrin;《2015 IEEE 6th Control and System Graduate Research Colloquium》;20160225;第55-60页 *
Design of obstacle avoidance controller for agricultural tractor based on ROS;Zhengduo Liu;《International Journal of Agricultural and Biological Engineering》;20191130;第12卷(第6期);第58-65页 *
农业机械导航技术研究进展;张漫;《农业机械学报》;20200430;第51卷(第4期);第1-18页 *
障碍物分类识别的果园机器人避障方法研究;耿乾;《中国农机化学报》;20200831;第41卷(第8期);第170-177页 *

Also Published As

Publication number Publication date
CN114637302A (zh) 2022-06-17

Similar Documents

Publication Publication Date Title
CN108955688B (zh) 双轮差速移动机器人定位方法及系统
CN110696818A (zh) 一种基于最优路径的自动泊车方法及系统
CN109976344B (zh) 巡检机器人姿态矫正方法
CN110243380B (zh) 一种基于多传感器数据与角度特征识别的地图匹配方法
CN110263607B (zh) 一种用于无人驾驶的道路级全局环境地图生成方法
JP2018536572A (ja) 障害物との衝突が切迫した状況におけるモーター式車両の操舵手段の制御システム
CN112183133B (zh) 一种基于ArUco码引导的移动机器人自主充电方法
WO2017154825A1 (ja) 車両位置認識システム
CN113311873B (zh) 一种基于视觉的无人机伺服跟踪方法
EP1548534B1 (en) Method and apparatus for using rotational movement amount of mobile device and computer-readable recording medium for storing computer program
CN111813114A (zh) 智能小车视觉导航方法
Han et al. Robust ego-motion estimation and map matching technique for autonomous vehicle localization with high definition digital map
CN114200945A (zh) 一种移动机器人的安全控制方法
CN115494849A (zh) 一种自动驾驶车辆导航控制方法及系统
CN113268065B (zh) 基于人工智能的agv自适应转弯避障方法、装置及设备
CN114637302B (zh) 一种基于计算机视觉的自动行进避障方法及系统
CN112099505B (zh) 一种低复杂度的移动机器人视觉伺服编队控制方法
Kang et al. Adaptive trajectory tracking of wheeled mobile robots based on a fish-eye camera
CN115993089B (zh) 基于pl-icp的在线四舵轮agv内外参标定方法
Lamon et al. 3D-Odometry for rough terrain-Towards real 3D navigation
CN115793676A (zh) 一种面向移动平台的旋翼无人机视觉引导降落方法
CN112947428B (zh) 一种四足机器人的移动控制方法及装置
TW202242580A (zh) 無人自走車之導引控制方法
US20200064849A1 (en) Trajectory planning method and system
Fachri et al. Multiple waypoint navigation for mobile robot using control lyapunov-barrier function (clbf)

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
GR01 Patent grant
GR01 Patent grant