CN111754577A - 目标识别系统及基于其的拖拉机倒车与农具连接方法 - Google Patents

Abstract

本发明提供了一个基于机器视觉的实时、准确的目标识别系统及相应的拖拉机倒车与农具连接方法，通过部署能够精确提取特征点的特殊目标图案或者标志物在农具上，用于目标的实时检测和相对位置计算；将摄像机成像仪中目标图案特征点带入至校正后的摄像机模型中，可精确推导出目标相对于摄像头的位置，即距离和方向角，由于摄像机与拖拉机相对静止，本发明可精确测量出的农机具和拖拉机各自连接点的相对位置和方向信息，计算规划导航路径，提供出建议车速与方向盘转角，以供驾驶员参考，以便手动或自动驾驶拖拉机到达农机具安装位置。相对于摄像机直接检测机械本身，本发明方法可极大的降低检测的计算量，提高准确性、可靠性和计算速度。

Description

目标识别系统及基于其的拖拉机倒车与农具连接方法

技术领域

本发明属于农机设备与计算机技术领域，涉及一种目标识别系统及基于其的拖拉机倒车与农具连接方法。

背景技术

在农业生产中，拖拉机常与农具配合使用来进行除草、喷药、耕地、浇水等工作，因此要做到合理安装与调整农具。农具的安装方式多种多样，以图1所示的三点悬挂装置为例，装置前端的三点固定在拖拉机后端，装置后端通过机构中间的通孔与农具进行连接。在安装时，螺栓要紧固连接带有通孔的悬挂装置和农具，因此要保证两个通孔的相对位置精确对准；其次，还要保证悬挂装置的连接杆顺利穿过，两孔的方向也要对准才能进行后续连接。由于拖拉机与农具体积重量都较大，实际生产中常常将农具水平放置在地面上，让拖拉机驾驶员低档小油门倒车至适当位置，使农具相应部位与拖拉机后端的三点悬挂装置对齐，方便后续进行安装，工作过程如图2所示。此过程精度要求较高（距离误差约±2cm，方向角误差约±1°），因此驾驶员要准确把握两个孔的实时位置和方向角关系，而驾驶员工作时通常是边观察车尾部的情况边倒车，操作比较困难，有经验的驾驶员一般也要来回十几次才能成功，且拖拉机体积越大，倒车时视野越差，增加了倒车难度。且仅凭肉眼观察待连接部位之间的位置和方向角关系，误差较大。

发明内容

为解决上述问题，本发明设计了一个基于机器视觉的实时、准确的目标识别系统及相应的拖拉机倒车与农具连接方法，能够辅助驾驶员手动倒车或实现拖拉机自动倒车，完成这一耦合过程，使驾驶员更快更精准地操作拖拉机倒车速度和转弯角度至安装位置。

本发明通过部署能够精确提取特征点的特殊目标图案或者标志物在农具上，用于目标的实时检测和相对位置计算；将摄像机成像仪中目标图案特征点带入至校正后的摄像机模型中，可精确推导出目标相对于摄像头的位置，即距离和方向角，由于摄像机与拖拉机相对静止，本发明可精确测量出农机具和拖拉机各自连接点的相对位置和方向信息，计算规划导航路径，提供出建议车速与方向盘转角，以供驾驶员参考，以便手动或自动驾驶拖拉机到达农机具安装位置。相对于摄像机直接检测机械本身，本发明方法可极大地降低检测的计算量，提高准确性、可靠性和计算速度。

为了达到上述目的，本发明提供如下技术方案：

拖拉机倒车与农具连接方法，包括如下步骤：

步骤1，建立摄像机模型及畸变校正

步骤1.1，建立摄像机模型如下：

(1)

其中，

；

上述公式中的(x y) ^T 代表的是物体在摄像机采集到的图像的像素坐标点；H是摄像机自身特性矩阵，描述了摄像机本身光学成像系统的特性；I _k 是物体相对于摄像机定义的坐标系中的坐标值；

步骤1.2，校正透镜畸变误差

将摄像头采集到的原始图像去除畸变，得到经透镜畸变校正后的图像上被映射点的坐标i’，真实世界中的点I’与校正后摄像机镜头拍摄画面中的点i’之间的映射关系，如公式(2)所示：

(2)

其中

，

；

上述公式中，i’中的(x’ y’) ^T 为经透镜畸变校正后的图像上被映射点的坐标，而I’中的(X Y Z) ^T 为物理点的世界坐标，R是描述世界坐标系与摄像机坐标系关系的3×4外部参数矩阵，是旋转矩阵S与平移向量u的组合，世界坐标系与摄像机坐标系之间的映射关系如公式(3)所示：

(3)

将公式(3)变形得公式(4)：

(4)

其中，旋转矩阵S表示了摄像机与目标之间绕三维坐标系x，y，z轴的旋转角度；而平移向量u代表了摄像机与目标在三维空间的平移关系，即三维空间各方向上的距离；

步骤2，计算位置关系

步骤2.1，在要连接的农具上设置目标图案；

步骤2.2，摄像机采集步骤2.1中设置的目标图案的原始图像，去除原始图像由摄像机光学成像系统带来的畸变，得到去畸变的图像；

步骤2.3，得到去畸变的图像后，检测提取到图案的若干特征点，得到各特征点在图像中的像素坐标点；

步骤2.4，根据公式(2)，输入摄像机的成像特性矩阵H，经校准后的特征点的像素坐标点i’，特征点在目标图案定义的自己的坐标系中的物理坐标I’，得到描述两个坐标系的相对关系的外部参数矩阵R；

步骤2.5，基于R得到旋转矩阵S与平移向量u，根据旋转矩阵S计算出目标图案对应摄像机在x、y、z三个方向的俯仰、偏航和滚转角，进而得到拖拉机和农机具的偏移角度关系，通过平移向量u得到两个坐标系的平移位置关系；将多个特征点在图像中的像素坐标点和自己在目标图案坐标系中的物理坐标代入公式(4)，得到目标图案相对于摄像机的位置关系；

步骤3，规划路径：

步骤3.1，设坐标系先绕X轴、再绕Y轴、最后绕Z轴分别旋转α _x 、α _y 和α _z 角得到旋转矩阵Rx、Ry和Rz，整个旋转矩阵R由三部分旋转矩阵相乘得到R=Rx*Ry*Rz，其中

整理后得到R与α _x 、α _y 和α _z 的关系为：

（5）

记拖拉机三点悬挂安装孔的坐标矩阵为

，拖拉机上的摄像头坐标矩阵为

，农机具上的目标图案坐标矩阵为

，农机具安装孔的坐标矩阵为

。由于在拖拉机上安装摄像头时已知拖拉机三点悬挂安装孔与摄像头在各轴上的偏移角度和平移位置关系，代入得R ₁₂ 与t ₁₂ ；

从而得拖拉机三点悬挂安装孔与拖拉机上摄像头的相对位置关系：

其中R₁₂为拖拉机三点悬挂安装孔与拖拉机上的摄像头之间的3×3旋转矩阵，t ₁₂ 为拖拉机三点悬挂安装孔与拖拉机上的摄像头之间的3×1平移向量；

由步骤2得拖拉机上的摄像头和农机具上的目标图案的相对位置关系为：

其中，R ₂₃ 为拖拉机上的摄像头和农机具上的目标图案之间的3×3旋转矩阵，t ₂₃ 为拖拉机上的摄像头和农机具上的目标图案之间的3×1平移向量；

得到目标图案和农机具安装孔的相对位置关系：

其中R ₃₄ 为目标图案和农机具安装孔之间的3×3旋转矩阵，t ₃₄ 为目标图案和农机具安装孔之间的3×1平移向量；

令

，

，

整理可得拖拉机三点悬挂安装孔和农机具安装孔的相对位置关系：

，

其中R’为拖拉机三点悬挂安装孔和农机具安装孔之间的3×3旋转矩阵，t’为拖拉机三点悬挂安装孔和农机具安装孔之间的3×1平移向量；

根据得到的拖拉机三点悬挂安装孔和农机具安装孔之间的旋转矩阵与平移向量，根据公式(5)计算出农机具安装孔对应拖拉机三点悬挂安装孔在x、y、z三个方向的俯仰、偏航和滚转角，通过平移向量u得到两个坐标系的平移位置关系；

步骤3.2，将拖拉机的农机具安装孔的位置定义为原点O’，车辆正前方的方向定义为y轴正方向，与y轴垂直且与地面平行指向车辆右侧的方向定义为x轴正方向，垂直地面向上的方向定义为z轴正方向，得到农机具上的安装孔P’在该坐标系中的坐标位置和方向作为驾驶系统规划轨迹的目标位置坐标和方向，拖拉机的农机具安装孔的位置和朝向在该定义的坐标系作为出发点的坐标和朝向；根据出发点O’和目标点P’的位置和朝向后，计算拖拉机倒车的导航轨迹；

步骤3.3，农机按照模拟出的导航轨迹行走，计算在每一小段路径上的转弯半径r和转向角度，计算机控制农机方向，使得农机按照导航轨迹行驶；取曲线弧度极小量l，通过下式计算：

其中，r为此段曲线的弧度半径，s为农机所需要行走的单位路径长度，θ为此农机行走此单位路径时方向盘需要转向的角度；

步骤3.4，车辆行驶速度保持某个范围值内低速倒车，同时车辆不停地实时探测导航目标图案，精确计算相对位置和方向数据，实时规划调整导航路径，得到车辆的目标转向角度或转弯半径；

步骤4，基于车辆与农具的相对位置关系，实时对车辆驾驶情况进行指导，在显示屏上显示车辆导航轨迹和操作所需信息；

当车辆倒车至车载摄像头实时拍摄的图案与预先拍摄的目标图案一致时，表示拖拉机已倒车至农具安装位置，结束指导。

进一步的，所述步骤2.1中农具上设置的图案形成于预先制作的图案板上，图案板安装在农具上。

进一步的，所述步骤3中特征点为黑格相交的点。

进一步的，所述步骤2.5中，目标图案对应摄像机在x、y、z三个方向的俯仰、偏航和滚转角通过对旋转矩阵S进行Rodrigues变换计算。

进一步的，所述步骤3.2中，导航轨迹通过以下过程计算：

将拖拉机的农机具安装孔的位置定义为出发点为O’，目标点为P’，y轴沿着拖拉机主轴方向，与拖拉机运动方向相同；拖拉机在任意一个时刻的姿态表述为(x, y, α)，其中α为y轴正向与前轮转角角度差，方向盘的转角为β，它与前轮的转角角度相同，设O’(0, 0, ψ)，P’(m, n, φ)，将路径分为三段，分别是从出发点右转到切线点1段的圆弧r ₁ ，从切线点1到切线点2段的直线s ₁ ，从切点2左转到目标点段的圆弧r ₂ ；进行坐标系转换，将原始的直角坐标系转换为，以点O’到点P’为x轴正方向，与地面平行且垂直于该方向向上为y轴正方向；

得到点O’指向点P’的方向ρ：

中间参数

；

单位化

，用弧度来表示路径长度，联立方程：

计算出r ₁ 、s ₁ 、r ₂ ，分别对应为三段路径。

进一步的，所述步骤4中，所述信息包括以下参数：建议车速、当前车速、目标坐标值、目标方向角、建议/实际车辆转弯半径和转向角度、待修正转弯偏差值；所述待修正转弯偏差值通过以下过程计算：将建议车辆转弯半径和转向角度与实际车辆转弯半径和转向角度分别相减，根据实际车辆转弯参数计算出与建议车辆转弯半径和转向角度的差值。

进一步的，所述步骤4中，当经过导航，坐标原点逐渐向目标点靠近时，车辆所需转角和车速均减小。

本发明还提供了一种目标识别系统，包括模型建立模块、位置关系计算模块、路径规划模块、屏幕显示模块；所述模型建立模块用于建立摄像机模型，再校正透镜畸变误差得到校正后的摄像机模型；所述位置关系计算模块用于采集目标图案的原始图像，去除畸变后得到去畸变的图像，检测提取到目标图案的所有特征点，基于校正后的摄像机模型，得到描述两个坐标系的相对关系的外部参数矩阵，计算出目标图案对应摄像机在x、y、z三个方向的俯仰、偏航和滚转角，得到拖拉机和农机具的偏移角度关系，通过平移向量得到两个坐标系的平移位置关系，基于多个特征点在图像中的像素坐标点和自己在目标图案坐标系中的物理坐标，计算得到目标图案相对于摄像机的位置关系；所述路径规划模块用于计算拖拉机上的安装孔和农机具安装孔的位置关系，根据两孔位置关系规划车辆行驶路径并计算转弯半径和转向角度，根据行驶路径在行驶过程中适时调整车辆各项参数；屏幕显示模块分析车辆与农具的相对位置关系，实时对车辆驾驶情况进行指导，将车辆行驶路线和相关信息显示在屏幕上，当车辆倒车至车载摄像头实时拍摄的图案与预先拍摄的目标图案一致时，结束指导。

进一步的，所述目标图案形成于图案板上，所述图案板安装在农具上。

进一步的，所述车辆各项参数至少包括：转弯半径、转向角度、车速。

与现有技术相比，本发明具有如下优点和有益效果：

1.本发明无需通过获得两车精确的地理位置（经纬度、海拔高度等）来计算拖拉机与农具的相对位置关系，巧妙地用目标识别定位系统代替卫星定位系统来进行相对定位，降低了成本，避免了因环境条件导致定位接收和发送信号中断、定位不准确等问题的出现。

2.通过间接识别图案板替代直接识别农机具来对农机具进行定位，简化了目标识别的数学模型，使得图像识别算法较易检测到目标并提取定位用的特征点，由此对农机具实现精确定位。

3.无需通过计算农机具安装孔与拖拉机的农机具安装孔二者精确的相对位置和方向角关系来判断拖拉机是否到达农具安装位置，而是通过计算摄像机坐标系与图案板坐标系的相对关系来判断的，避免了再去根据摄像头的安装情况等因素来精确计算目标点的最终位置，简化了计算步骤，提高了系统的稳定性。

4.由于农机具所携带的物料（例如除草机、杀虫剂等）通常是对人体有害的，利用该系统可精确指导拖拉机倒车至安装位置时符合连接要求，可启动装置直接进行自动连接，无需人员再牵引农具或手动拧螺栓来进行细致调整，减少了人员和农机具上有害物质的接触，同时也实现了精确对准自动连接，提高了作业自动化水平。

5.本发明方法具有较好的稳定性和抗干扰性，能够实时检测到连接机构与农具的相对位置和方向角的信息，通过可靠算法分析行驶路线，且误差范围较小（位置误差±2cm，方向角误差±1°），尤其适用于对精度要求较高的工作，能够更好地实现农业自动化。

附图说明

图1为三点悬挂装置结构图。

图2为拖拉机倒车过程示意图。

图3为理想状态下图案板示意图，其中（a）为目标图案示意图，（b）为提取特征点示意图，（c）为各特征点坐标图。

图4为实际图案板示意图，其中（a）为图案板图案示意图，（b）为经校正后的各特征点坐标。

图5为连接状态下拖拉机与农机具示意图，其中（a）为连接状态下拖拉机与农机具示意图，（b）为连接状态下用摄像头拍摄到的照片，其中背景已过滤。

图6为显示屏显示图。

图7为显示屏显示图，其中方向角已标出。

图8为显示屏显示图，其中拖拉机上的农机具安装孔已逐渐向目标点靠近。

图9为显示屏显示图，其中车辆已到达目标位置。

图10为拖拉机从起点到目标点倒车路线规划示意图。

图11为取极小曲线弧度时，弧度半径、弧长和弧长对应的圆心角三者的近似关系。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。本发明步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

由于农机具通常结构复杂，若利用摄像头直接捕获农机具来对其进行检测识别，难以建立精确的数学模型，且计算量大，识别成功率低，很难做到精准定位。本发明间接通过在要挂接的农具上喷涂特殊图案或在农具上额外安装一个图案板，利用拖拉机搭载的摄像头捕获目标图案来代替直接捕获农机具。由于自制图案板不受限于某种固定模板，且相较于农机具识别特征明显，摄像机可以精准地捕获到目标图形，根据实际采集到的目标图形的大小和形变，该系统可计算出待连接部位之间相关的距离和方位角的关系，然后将处理后的数据发送到拖拉机驾驶室的屏幕上，以图像的方式显示测量的信息，驾驶员可在显示屏上实时观察到拖拉机与连接部位的距离和方向等关系，根据系统规划的合适倒车路径来手动倒车。显示屏上可显示规划的转向角度（或转弯半径）和实际转向角度（或转弯半径），并能实时显示与目标物体偏离的角度和距离，规划路径也在根据实际车辆情况实时更新，驾驶员不必再多次调头看连接情况。该方法也可实现自动倒车，根据摄像头反馈的信息，系统可规划行驶路径，自动控制车轮或履带来进行转向，使拖拉机快速准确地倒车，驾驶员可通过显示屏观察自动倒车情况。本系统能更精准地引导驾驶员手动倒车或自动倒车至安装位置，快速地实现连接部位正确安装。

具体的说，本发明提供的拖拉机倒车与农具连接方法包括如下步骤：

步骤1，建立摄像机模型及畸变校正

步骤1.1，建立摄像机模型。本目标识别定位系统中采用的是针孔模型。公式(1)描述了现实世界中的点与投影到摄像机拍摄的图像中的点的对应关系。

(1)

其中

上述公式中的(x y) ^T 代表的是物体在摄像机采集到的图像的像素坐标点；H是摄像机自身特性矩阵，描述了摄像机本身光学成像系统的特性，该参数与下述的畸变参数可通过输入在不同视角捕获的目标图案的特征点来得到；I _k 是物体相对于摄像机定义的坐标系中的坐标值。

步骤1.2，校正透镜畸变误差

公式1的针孔模型是一个理想模型。实际的摄像头光学系统的成像会有畸变，并不完全满足针孔模型，因此需要校准摄像头光学系统的畸变参数，将摄像头采集到的原始图像去除畸变，得到经透镜畸变校正后的图像上被映射点的坐标i’，真实世界中的点I’与校正后摄像机镜头拍摄画面中的点i’之间的映射关系，如公式(2)所示。

(2)

其中

，

；

上述公式中，i’中的(x’ y’) ^T 为经透镜畸变校正后的图像上被映射点的坐标，而I’中的(X Y Z) ^T 为一个物理点的世界坐标。R是描述世界坐标系与摄像机坐标系关系的3×4外部参数矩阵，是旋转矩阵S与平移向量u的组合，世界坐标系与摄像机坐标系之间的映射关系如公式(3)所示。

(3)

将公式(3)变形可得公式(4)：

(4)

本发明中，世界坐标系统是由目标定义的坐标系统，公式(4)中的摄像机与目标之间的位置关系即可由公式(4)中的旋转矩阵S和平移向量u表示。其中旋转矩阵S表示了摄像机与目标之间绕三维坐标系x, y, z轴的旋转角度；而平移向量u代表了摄像机与目标在三维空间的平移关系，即三维空间各方向上的距离。

步骤2，计算位置关系

步骤2.1，制作图案板。

图案板可以是平面的，也可以是有立体构型的。图案板上的图案可以绘制在图案板的一个平面上，也可以覆盖图案板的多个平面形成立体图形。选取图案板的某个特定点为图案板自己定义的坐标系的原点，再定义一下图案板确定的坐标系的方向。以图3所示棋盘格模板图案为例，该图案易与背景环境相区分，便于摄像机提高定标速度和可靠性，缩短定标时间；利用其由栅格线构成的结构特征，选取黑格相交的点为特征点（如图3（b）所示），这样有利于数学模型对所得特征点进行直线拟合，更加迅速准确地确定各特征点的坐标。该图案坐标系定义如图，根据已知的图案板制作尺寸，就可得到图案板所有特征点在自己定义的坐标系的坐标位置，如图3（c）所示。图案板安装在农具上，图案板上的图案称之为目标图案。我们也可以根据需要，直接在农具上绘制图案。

用于定位的目标图案可有如下多种实现模式：

1.具有特殊图案模式的图像，与复杂的背景环境区别较大，易于识别。

2.具有特殊的颜色，与机械和周围环境区别很大，比如红色等。针对不同的品牌机械的颜色和工作环境进行调整。

3.一组按照一定特殊频率节奏闪烁的点光源，也易于被摄像机和机器视觉算法识别和检测到，通过检测光源并精确计算它们在图片中的位置，也可实现定位。

4.一组按照一定特殊频率节奏闪烁的图案，也易于被摄像机和机器视觉算法从背景图像中分割出来。从背景图像中分割出来的用于定位的目标图案将被进一步提取特征点在图像中的像素位置，并用于定位的计算。

5.一组可以发射特定波长的光形成的光点（可为主动发射光的光源，或者可透过或反射相应波长的光）。通过在摄像机镜头安装只透过该波长范围内的滤光片，即可实现对作为定位特征点的光点的实时可靠检测。

6.一组可发射特定波长的光图案（可为主动发射光的光源，或者可透过或反射相应波长的光），也易于被摄像机和机器视觉算法从背景图像中分割出来。从背景图像中分割出来的用于定位的目标图案将被进一步提取特征点在图像中的像素位置，并用于定位的计算。

步骤2.2，摄像机采集实际图案板的原始图像（图4（a）），利用上述步骤1得到的摄像机自身特性矩阵和畸变参数等数据去除原始图像由摄像机光学成像系统带来的畸变，得到去畸变的图像。

步骤2.3，得到去畸变的图像后，检测提取到图案板的所有特征点，得到各特征点在图像中的像素坐标点，所提取的特征点数量越多，对后续的目标定位越精确。

步骤2.4，输入公式(2)，得到描述两个坐标系的相对关系的外部参数矩阵R。输入的包括，摄像机的成像特性矩阵（校准得到）H，经校准后的特征点的像素坐标点i’，特征点在图案板定义的自己的坐标系中的物理坐标I’。

步骤2.5，由于

，得到R也就得到了旋转矩阵S与平移向量u，对旋转矩阵S进行Rodrigues变换计算出目标图案对应摄像机在x、y、z三个方向的俯仰、偏航和滚转角，进而得到拖拉机和农机具的偏移角度关系，通过平移向量u可得到两个坐标系的平移位置关系。将多个特征点在图像中的像素坐标点和自己在图案板坐标系中的物理坐标带入公式(4)，即可解方程，得到目标图案相对于摄像机的位置关系。

步骤3，规划路径：

步骤3.1，设坐标系先绕X轴、再绕Y轴、最后绕Z轴分别旋转α _x 、α _y 和α _z 角得到旋转矩阵Rx、Ry和Rz，整个旋转矩阵R由三部分旋转矩阵相乘得到R=Rx*Ry*Rz，其中

整理后得到R与α _x 、α _y 和α _z 的关系为：

（5）

记拖拉机三点悬挂安装孔的坐标矩阵为

，拖拉机上的摄像头坐标矩阵为

，农机具上的图案板坐标矩阵为

，农机具安装孔的坐标矩阵为

。由于在拖拉机上安装摄像头时已知拖拉机三点悬挂安装孔与摄像头在各轴上的偏移角度和平移位置关系，代入可得R ₁₂ 与t ₁₂ ，

从而可得拖拉机三点悬挂安装孔与拖拉机上摄像头的相对位置关系：

其中R₁₂为拖拉机三点悬挂安装孔与拖拉机上的摄像头之间的3×3旋转矩阵，t ₁₂ 为拖拉机三点悬挂安装孔与拖拉机上的摄像头之间的3×1平移向量。

由步骤2可得拖拉机上的摄像头和农机具上的图案板的相对位置关系为：

其中，R ₂₃ 为拖拉机上的摄像头和农机具上的图案板之间的3×3旋转矩阵，t ₂₃ 为拖拉机上的摄像头和农机具上的图案板之间的3×1平移向量。

由于在农机具上安装图案板时也已知图案板与农机具安装孔在各轴上的偏移角度和平移位置关系，代入可得R ₁₂ 与t ₁₂ ，

从而可得图案板和农机具安装孔的相对位置关系：

其中R ₃₄ 为图案板和农机具安装孔之间的3×3旋转矩阵，t ₃₄ 为图案板和农机具安装孔之间的3×1平移向量。

令

，

，

整理可得拖拉机三点悬挂安装孔和农机具安装孔的相对位置关系：

，

其中R’为拖拉机三点悬挂安装孔和农机具安装孔之间的3×3旋转矩阵，t’为拖拉机三点悬挂安装孔和农机具安装孔之间的3×1平移向量。

根据得到的拖拉机三点悬挂安装孔和农机具安装孔之间的旋转矩阵与平移向量，根据公式(5)计算出农机具安装孔对应拖拉机三点悬挂安装孔在x、y、z三个方向的俯仰、偏航和滚转角，通过平移向量u可得到两个坐标系的平移位置关系。

步骤3.2，现将拖拉机的农机具安装孔的位置定义为原点O’，车辆正前方的方向定义为y轴正方向，与y轴垂直且与地面平行指向车辆右侧的方向定义为x轴正方向，垂直地面向上的方向定义为z轴正方向，即可得到农机具上的安装孔P’在该坐标系中的坐标位置和方向，它作为驾驶系统规划轨迹的目标位置坐标和方向。拖拉机的农机具安装孔的位置和朝向在该定义的坐标系作为出发点的坐标和朝向。知道出发点O’和目标点P’的位置和朝向后，计算拖拉机倒车的导航轨迹。

导航轨迹可根据现有技术中的很多成熟算法得到，本例给出了以下一种可行方式以作示例：拖拉机车辆运动模型可将拖拉机看做平面上的刚体运动，将拖拉机的农机具安装孔的位置定义为出发点为O’，目标点为P’，y轴沿着拖拉机主轴方向，与拖拉机运动方向相同；拖拉机在任意一个时刻的姿态可以表述为(x, y, α)（α为y轴正向与前轮转角角度差），方向盘的转角为β，它与前轮的转角角度相同。如图10所示，设O’(0, 0, ψ)，P’(m, n, φ)，将路径分为三段，分别是从出发点右转到切线点1段的圆弧r ₁ ，从切线点1到切线点2段的直线s ₁ ，从切点2左转到目标点段的圆弧r ₂ 。进行坐标系转换，将原始的直角坐标系转换为，以点O’到点P’为x轴正方向，与地面平行且垂直于该方向向上为y轴正方向，

可得：

点O’指向点P’的方向ρ：

中间参数

单位化

，用弧度来表示路径长度，联立方程：

由计算机解得方程计算出r ₁ 、s ₁ 、r ₂ ，分别对应为三段路径。

步骤3.3，前述步骤中，根据初始点和终点位置和农机模型，计算机模拟出拖拉机实际行走的导航轨迹，通常为曲线。当农机按照模拟出轨迹行走时，如图11所示，取曲线弧度极小量l，通过下式计算：

其中l为曲线所选取的弧度极小量长度，r为此段曲线的弧度半径，s为农机所需要行走的单位路径长度，θ为此农机行走此单位路径时方向盘需要转向的角度。在农机行进时，已知θ，通过计算在每一小段路径上的转弯半径r和转向角度，计算机对农机方向加以控制，确保农机严格按照导航轨迹行驶至终点。转向角度可以通过常规方法计算。

步骤3.4，在这个过程中，车辆行驶速度保持某个范围值内低速倒车，同时车辆不停地实时探测（每间隔一小段时间——如间隔0.1秒即探测）导航图案板，精确计算相对位置和方向数据，实时规划调整导航路径，得到车辆的目标转向角度或转弯半径，用以辅助驾驶员手动倒车或者控制系统自动倒车。

步骤4，屏幕显示：

步骤4.1，用户应事先将拖拉机与农机具连接好，记录在已连接状态下车载摄像头拍摄到的照片，如图5(a)所示，通过照片中捕获到的图案板计算出摄像机坐标系与图案板坐标系的变换关系并永久保存，以供在之后的作业中连接农具使用。在拖拉机倒车的过程中，如果摄像机拍摄的照片中捕获到的图案板与此次相同，则代表拖拉机已倒车至农具安装位置。

步骤4.2，系统经过分析车辆与农具的相对位置关系，实时对车辆驾驶情况进行指导，规划车辆行驶路线。系统将信息显示到显示屏上，如图6所示以车辆正前方作为屏幕的正上方（因为驾驶员视野通常为车辆正前方），将系统预估轨迹与实际操作的方向盘转角、车速等信息实时显示在屏上。

显示屏中显示以下主要参数：

①建议车速：驾驶系统规划路径时建议车辆行驶速度的范围值，超出该范围时车辆实际行驶路线可能会与导航路线有较大偏差；如2-4km/h表示车速保持在2-4km/h范围内时，能够精确循着导航路线行驶；

②当前车速：车辆当前的行驶速度，如3.643km/h，当前车速在建议车速范围内，符合要求；

③目标坐标值：表示目标（即农机具）在由拖拉机定义的坐标系中的坐标值，用来表示目标与拖拉机的相对位置关系；如（-2.529，-3.732）表示目标位于车辆的左后方，前后方向距离为3.732米，左右方向距离为2.529米

④目标方向角：以上述定义的y轴正向为0°朝向，顺时针方向增加，用来表示目标与拖拉机二者之间待连接通孔的角度差（角度差在±1°范围内时符合连接要求），如10.327°的含义如图7所示。

⑤/⑥建议/实际车辆转弯半径和转向角度：导航路线规划的车辆转弯半径和转向角度，以指导驾驶员转向或调整车辆完成自动转向；

⑦待修正转弯偏差值：将建议车辆转弯半径和转向角度与实际车辆转弯半径和转向角度分别相减，根据实际车辆转弯参数计算出与建议车辆转弯半径和转向角度的差值，以指导驾驶员修正当前参数至建议转弯半径和转向角度或调整车辆完成自动转向；

车辆实际数据（车速、实际转弯半径和转向角度）由系统实时采集。

步骤4.3，经过一段时间的导航，可观察到显示屏中农具与车辆在前后距离、左右距离上均不断缩小，即坐标原点（拖拉机上的农机具安装孔）逐渐向目标点靠近，此时车辆所需转角和车速也均减小，如图8所示。

步骤4.4，当车辆倒车至满足上述步骤4.1中摄像头坐标系与图案板坐标系变换关系的位置时，表示拖拉机已倒车至农具安装位置，如图9所示，自动倒车系统结束指导。此时待连接通孔的位置和方向角差均在误差允许范围内，作业人员可启用自动连接装置连接车辆后端和农机具装置的通孔，将农具固定在车辆后端，完成农具的安装。

本发明还提供了能够实现上述方法的目标识别系统，包括模型建立模块、位置关系计算模块、路径规划模块、屏幕显示模块。本系统采用软件实现。模型建立模块用于首先建立摄像机模型，再校正透镜畸变误差得到校正后的摄像机模型，具体实现步骤1中的内容；位置关系计算模块用于采集实际图案板的原始图像，去除畸变后得到去畸变的图像，检测提取到图案板的所有特征点，基于校正后的摄像机模型，得到描述两个坐标系的相对关系的外部参数矩阵，计算出目标图案对应摄像机在x、y、z三个方向的俯仰、偏航和滚转角，得到拖拉机和农机具的偏移角度关系，通过平移向量可得到两个坐标系的平移位置关系，基于多个特征点在图像中的像素坐标点和自己在图案板坐标系中的物理坐标，计算得到目标图案相对于摄像机的位置关系，具体实现步骤2中的内容；路径规划模块用于计算拖拉机上的安装孔和农机具安装孔的位置关系，根据两孔位置关系规划车辆行驶路径并计算转弯半径和转向角度，根据行驶路径在行驶过程中适时调整车辆各项参数（各项参数至少包括转弯半径、转向角度和车速），具体实现步骤3中的内容；屏幕显示模块分析车辆与农具的相对位置关系，实时对车辆驾驶情况进行指导，将车辆行驶路线和相关信息显示在屏幕上，当车辆倒车至车载摄像头实时拍摄的图案板与预先拍摄的标准图片一致时，结束指导，具体实现步骤4中的内容。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (10)

1.拖拉机倒车与农具连接方法，其特征在于，包括如下步骤：

步骤1，建立摄像机模型及畸变校正

步骤1.1，建立摄像机模型如下：

(1)

其中，

；

上述公式中的(x y) ^T 代表的是物体在摄像机采集到的图像的像素坐标点；H是摄像机自身特性矩阵，描述了摄像机本身光学成像系统的特性；I _k 是物体相对于摄像机定义的坐标系中的坐标值；

步骤1.2，校正透镜畸变误差

将摄像头采集到的原始图像去除畸变，得到经透镜畸变校正后的图像上被映射点的坐标i’，真实世界中的点I’与校正后摄像机镜头拍摄画面中的点i’之间的映射关系，如公式(2)所示：

(2)

其中

，

；

上述公式中，i’中的(x’ y’) ^T 为经透镜畸变校正后的图像上被映射点的坐标，而I’中的(X Y Z) ^T 为物理点的世界坐标，R是描述世界坐标系与摄像机坐标系关系的3×4外部参数矩阵，是旋转矩阵S与平移向量u的组合，世界坐标系与摄像机坐标系之间的映射关系如公式(3)所示：

(3)

将公式(3)变形得公式(4)：

(4)

其中，旋转矩阵S表示了摄像机与目标之间绕三维坐标系x，y，z轴的旋转角度；而平移向量u代表了摄像机与目标在三维空间的平移关系，即三维空间各方向上的距离；

步骤2，计算位置关系

步骤2.1，在要连接的农具上设置目标图案；

步骤2.2，摄像机采集步骤2.1中设置的目标图案的原始图像，去除原始图像由摄像机光学成像系统带来的畸变，得到去畸变的图像；

步骤2.3，得到去畸变的图像后，检测提取到图案的若干特征点，得到各特征点在图像中的像素坐标点；

步骤2.4，根据公式(2)，输入摄像机的成像特性矩阵H，经校准后的特征点的像素坐标点i’，特征点在目标图案定义的自己的坐标系中的物理坐标I’，得到描述两个坐标系的相对关系的外部参数矩阵R；

步骤2.5，基于R得到旋转矩阵S与平移向量u，根据旋转矩阵S计算出目标图案对应摄像机在x、y、z三个方向的俯仰、偏航和滚转角，进而得到拖拉机和农机具的偏移角度关系，通过平移向量u得到两个坐标系的平移位置关系；将多个特征点在图像中的像素坐标点和自己在目标图案坐标系中的物理坐标代入公式(4)，得到目标图案相对于摄像机的位置关系；

步骤3，规划路径：

步骤3.1，设坐标系先绕X轴、再绕Y轴、最后绕Z轴分别旋转α _x 、α _y 和α _z 角得到旋转矩阵Rx、Ry和Rz，整个旋转矩阵R由三部分旋转矩阵相乘得到R=Rx*Ry*Rz，其中

整理后得到R与α _x 、α _y 和α _z 的关系为：

（5）

记拖拉机三点悬挂安装孔的坐标矩阵为

，拖拉机上的摄像头坐标矩阵为

，农机具上的目标图案坐标矩阵为

，农机具安装孔的坐标矩阵为

；由于在拖拉机上安装摄像头时已知拖拉机三点悬挂安装孔与摄像头在各轴上的偏移角度和平移位置关系，代入得R ₁₂ 与t ₁₂ ；

从而得拖拉机三点悬挂安装孔与拖拉机上摄像头的相对位置关系：

其中R₁₂为拖拉机三点悬挂安装孔与拖拉机上的摄像头之间的3×3旋转矩阵，t ₁₂ 为拖拉机三点悬挂安装孔与拖拉机上的摄像头之间的3×1平移向量；

由步骤2得拖拉机上的摄像头和农机具上的目标图案的相对位置关系为：

其中，R ₂₃ 为拖拉机上的摄像头和农机具上的目标图案之间的3×3旋转矩阵，t ₂₃ 为拖拉机上的摄像头和农机具上的目标图案之间的3×1平移向量；

得到目标图案和农机具安装孔的相对位置关系：

其中R ₃₄ 为目标图案和农机具安装孔之间的3×3旋转矩阵，t ₃₄ 为目标图案和农机具安装孔之间的3×1平移向量；

令

，

，

整理可得拖拉机三点悬挂安装孔和农机具安装孔的相对位置关系：

，

其中R’为拖拉机三点悬挂安装孔和农机具安装孔之间的3×3旋转矩阵，t’为拖拉机三点悬挂安装孔和农机具安装孔之间的3×1平移向量；

根据得到的拖拉机三点悬挂安装孔和农机具安装孔之间的旋转矩阵与平移向量，根据公式(5)计算出农机具安装孔对应拖拉机三点悬挂安装孔在x、y、z三个方向的俯仰、偏航和滚转角，通过平移向量u得到两个坐标系的平移位置关系；

步骤3.2，将拖拉机的农机具安装孔的位置定义为原点O’，车辆正前方的方向定义为y轴正方向，与y轴垂直且与地面平行指向车辆右侧的方向定义为x轴正方向，垂直地面向上的方向定义为z轴正方向，得到农机具上的安装孔P’在该坐标系中的坐标位置和方向作为驾驶系统规划轨迹的目标位置坐标和方向，拖拉机的农机具安装孔的位置和朝向在该定义的坐标系作为出发点的坐标和朝向；根据出发点O’和目标点P’的位置和朝向后，计算拖拉机倒车的导航轨迹；

步骤3.3，农机按照模拟出的导航轨迹行走，计算在每一小段路径上的转弯半径r和车轮转向角度，计算机控制农机方向，使得农机按照导航轨迹行驶；取曲线弧度极小量l，通过下式计算：

其中，r为此段曲线的弧度半径，s为农机所需要行走的单位路径长度，θ为此农机行走此单位路径时方向盘需要转向的角度；

步骤3.4，车辆行驶速度保持某个范围值内低速倒车，同时车辆不停地实时探测导航目标图案，精确计算相对位置和方向数据，实时规划调整导航路径，得到车辆的目标转向角度或转弯半径；

步骤4，基于车辆与农具的相对位置关系，实时对车辆驾驶情况进行指导，在显示屏上显示车辆导航轨迹和操作所需信息；

当车辆倒车至车载摄像头实时拍摄的图案与预先拍摄的目标图案一致时，表示拖拉机已倒车至农具安装位置，结束指导。

2.根据权利要求1所述的拖拉机倒车与农具连接方法，其特征在于，所述步骤2.1中农具上设置的图案形成于预先制作的图案板上，图案板安装在农具上。

3.根据权利要求1所述的拖拉机倒车与农具连接方法，其特征在于，所述步骤3中特征点为黑格相交的点。

4.根据权利要求1所述的拖拉机倒车与农具连接方法，其特征在于，所述步骤2.5中，目标图案对应摄像机在x、y、z三个方向的俯仰、偏航和滚转角通过对旋转矩阵S进行Rodrigues变换计算。

5.根据权利要求1所述的拖拉机倒车与农具连接方法，其特征在于，所述步骤3.2中，导航轨迹通过以下过程计算：

将拖拉机的农机具安装孔的位置定义为出发点为O’，目标点为P’，y轴沿着拖拉机主轴方向，与拖拉机运动方向相同；拖拉机在任意一个时刻的姿态表述为(x, y, α)，其中α为y轴正向与前轮转角角度差，方向盘的转角为β，它与前轮的转角角度相同，设O’(0, 0, ψ)，P’(m, n, φ)，将路径分为三段，分别是从出发点右转到切线点1段的圆弧r ₁ ，从切线点1到切线点2段的直线s ₁ ，从切点2左转到目标点段的圆弧r ₂ ；进行坐标系转换，将原始的直角坐标系转换为，以点O’到点P’为x轴正方向，与地面平行且垂直于该方向向上为y轴正方向；

得到点O’指向点P’的方向ρ：

中间参数

；

单位化

，用弧度来表示路径长度，联立方程：

计算出r ₁ 、s ₁ 、r ₂ ，分别对应为三段路径。

6.根据权利要求1所述的拖拉机倒车与农具连接方法，其特征在于所述步骤4中，所述信息至少包括以下参数：建议车速、当前车速、目标坐标值、目标方向角、建议/实际车辆转弯半径和转向角度、待修正转弯偏差值；所述待修正转弯偏差值通过以下过程计算：将建议车辆转弯半径和转向角度与实际车辆转弯半径和转向角度分别相减，根据实际车辆转弯参数计算出与建议车辆转弯半径和转向角度的差值。

7.根据权利要求1所述的拖拉机倒车与农具连接方法，其特征在于，所述步骤4中，当经过导航，坐标原点逐渐向目标点靠近时，车辆所需转角和车速均减小。

8.一种目标识别系统，其特征在于，包括模型建立模块、位置关系计算模块、路径规划模块、屏幕显示模块；所述模型建立模块用于建立摄像机模型，再校正透镜畸变误差得到校正后的摄像机模型；所述位置关系计算模块用于采集目标图案的原始图像，去除畸变后得到去畸变的图像，检测提取到目标图案的所有特征点，基于校正后的摄像机模型，得到描述两个坐标系的相对关系的外部参数矩阵，计算出目标图案对应摄像机在x、y、z三个方向的俯仰、偏航和滚转角，得到拖拉机和农机具的偏移角度关系，通过平移向量可得到两个坐标系的平移位置关系，基于多个特征点在图像中的像素坐标点和自己在目标图案坐标系中的物理坐标，计算得到目标图案相对于摄像机的位置关系；所述路径规划模块用于计算拖拉机上的安装孔和农机具安装孔的位置关系，根据两孔位置关系规划车辆行驶路径并计算转弯半径和转向角度，根据行驶路径在行驶过程中适时调整车辆各项参数；屏幕显示模块分析车辆与农具的相对位置关系，实时对车辆驾驶情况进行指导，将车辆行驶路线和相关信息显示在屏幕上，当车辆倒车至车载摄像头实时拍摄的图案与预先拍摄的目标图案一致时，结束指导。

9.根据权利要求8所述的目标识别系统，其特征在于，所述目标图案形成于图案板上，所述图案板安装在农具上。

10.根据权利要求8所述的目标识别系统，其特征在于，所述车辆各项参数至少包括：转弯半径、转向角度、车速。

Images