一种无人机视觉追踪实现方法与流程

本发明涉及一种无人机视觉追踪实现方法。

背景技术：

随着无人机技术越来越成熟，无人机的应用领域也越来越广泛，近两年除消费级无人机火爆以外，无人机在其他很多行业也得到广泛发展，如物流无人机、航拍无人机、侦查无人机等，无人机在执行任务过程中需要跟踪目标物，目前无人机定位采用gps技术，由于gps信号受遮挡、天气干扰严重，不能实现精确定位。

技术实现要素：

本发明的目的在于提供一种无人机视觉追踪实现方法，解决现有技术中无人机视觉追踪移动目标物不能实现精确定位的技术问题。

为了解决上述问题，本发明采取如下技术方案：

一种无人机视觉追踪实现方法，包括如下步骤：

1)、进行无人机摄像机标定、获取摄像机内参矩阵；

2)、无人机起飞，摄像机拍摄图像，无人机控制器获取图像数据并通过无线通讯模块实时发送给地面站控制系统；

3)、地面站控制系统实时检测接收的图像数据，并判断被追踪目标是否出现当前图像中，即摄像机视野中：

3.1)、如果否，则地面站控制系统遥控摄像机的云台，再次拍摄图像，并传递给地面站控制系统检测，所述云台为三轴云台；

3.2)、如果是，则地面站控制系统在当前图像中框选出被追踪目标，并将框选出的被追踪目标数据传递给无人机；

4)、无人机控制器对框选出的被追踪目标数据进行校验，判断框选出的被追踪目标数据是否有效：

4.1)、如果无效，则将校验失败的结果传递给地面站控制系统，传递给地面站控制系统重新在当前图像中框选被追踪目标；

4.2)、如果有效，则无人机进行被追踪目标识别，并判断是否成功识别到框选出的被追踪目标；

4.2.1)、如果失败，则将识别失败的结果传递给地面站控制系统，传递给地面站控制系统重新在当前图像中框选被追踪目标；

4.2.2)、如果成功，则无人机控制器根据图像识别输出的像素偏差控制云台对准待追踪目标；

5)、判断追踪目标是否位于图像中心：

5.1)如果否，则无人机控制器重新根据当前图像识别输出的像素偏差控制云台对准待追踪目标；

5.2)如果是，则获取此时云台的姿态角姿态角其中，ψ为滚转角，θ为俯仰角，为偏航角，无人机控制器根据姿态角控制无人机继续飞行；根据无人机上设置的加速度计、陀螺仪、磁传感器可以直接得出云台姿态角；

6)、判断无人机是否飞行至追踪目标正上方：

6.1)如果否、则控制器继续根据此时云台的姿态角控制无人机继续飞行；

6.2)如果是，则无人机达到机视觉追踪的目的。

进一步改进，获取相机的内参矩阵的步骤如下；

1.1)、建立相机坐标系、成像平面坐标系、像素坐标系；

相机坐标系：以相机光心oc作为原点，ocxc轴平行与成像平面的水平方向，从相机的后方看，指向相机的右方，ocyc轴平行于成像平面竖直方向，指向相机下方，光轴oczc垂直与xcocyc平面；

像素坐标系：为一个二维直角坐标系，以像素为单位，以图像的左上点为原点o，ou轴与图像宽度方向平行，沿图像上顶边指向右侧，ov轴与图像高度方向平行，沿图像左边界指向下方。

成像平面坐标系：为一个二维直角坐标系，以图像中心oi为原点，该点为相机光轴与成像平面的交点，oixi轴与ocxc轴平行，oiyi轴与ocyc轴平行，并且两者的正方向相同；

1.2)完成相机坐标系、成像平面坐标系、像素坐标系之间的转换，

1.2.1)、将相机坐标系转换到成像平面坐标系：

设点a(x，y，z)为相机坐标系空间中的一点，则a(x，y，f)点为a点在到图像平面上的投影，相机的焦距为f，得出：

x/f＝x/z，y/f＝y/z；

即，x＝fx/z，y＝fy/z；

上述变换关系用3*3的矩阵表示为：q＝mq，其中

得出透视投影变换矩阵为：

1.2.2)、成像平面坐标系转换到像素坐标系：

设定成像平面坐标的原点oi在以像素为单位的成像平面坐标中的坐标为(u0，v0)；设每个像素的物理尺寸大小为dx*dy(mm)，dx≠dy；

设定图像平面上某点在成像平面坐标系中的坐标为(x，y)，在像素坐标系中的坐标为(u,v)，则二者满足如下关系：

u＝x/dx+u0；v＝y/dy+v0；

用齐次坐标与矩阵形式表示为：

将等式两边都乘以z可得：

将相机坐标系中的(1)式代入上式可得：

则获得相机的内参矩阵：

进一步改进，所述步骤5.1)中，建立成像坐标系：为一个二维直角坐标系，以图像中心oi为原点，该点由相机光轴与图像平面的交点，oixi轴水平，向右为正方向，oiyi轴垂直，向上为正方向；设定px为此刻所拍摄图像在oixi方向的像素偏差值，py为在oiyi方向的像素偏差值，控制器以kpx、hpy作为进行云台闭环控制的输入量，其中，k为oixi方向云台追踪目标的速度系数，h为为oiyi方向云台追踪目标的速度系数，kpx用于云台的偏航角控制，hpy用于云台的俯仰角控制。

k、h值越大追踪速度越快，但是如果太大有可能出现振荡。因此需要k、h值需要实际的应用场景调节。kpx用于云台的偏航角控制，hpy用于云台的俯仰角控制。kpx、hpy是以o为坐标原点，所以能够产生一个闭环控制量，控制云台对准待追踪目标。

进一步改进，所述步骤5.2)中，无人机获取此时云台的姿态角其中，ψ为滚转角，θ为俯仰角，为偏航角，控制器根据姿态角控制无人机飞行追踪目标物包括两种策略：一种为使用云台的姿态角作为输入量进行控制；另一种为根据目标物和无人机在地面坐标系中的坐标偏差作为输入量控制无人机飞行。

进一步改进，所述使用云台的姿态角作为输入量控制无人机的方法如下：计算无人机偏航角与云台偏航角差，以及无人机俯仰角与云台俯仰角差，判断偏航角差是否等于0度：

1)、如果不等于0，则控制无人机改变自身姿态，向减小偏航角差的方向旋转，并再次判断；

2)如果等于0，则判断俯仰角差是否等于90度：

2.1)、如果不等于90度，则控制无人机改变自身姿态，向减小俯仰角差的方向飞行并再次判断；

2.2)、如果等于90度，则无人机位于追踪目标正上方，然后判断追踪任务是否结束：

2.2.1)、如果否，则使用云台的当前姿态角作为输入量控制无人机继续飞行；

2.2.2)、如果是，无人机达到机视觉追踪的目的。

进一步改进，所述根据目标物和无人机在地面坐标系中的坐标偏差作为输入量控制无人机飞行的步骤如下：

1)、建立地面坐标系、机体坐标系、云台坐标系、相机坐标系、像素坐标系和成像坐标系：

地面坐标系：原点og为旋翼无人机的起飞点，ogxg轴在地平面内指向地球北极或旋翼无人机的前飞方向，ogzg轴垂直与地平面朝下，ogyg轴垂直于xgogzg平面，正方指向右侧；

机体坐标系：ob为旋翼无人机中心，obxb轴指向机体正前方，obyb轴指向机体右侧，obzb轴垂直与xbobyb平面并指向机体下方；

云台坐标系：定义云台三个旋转轴的交点为云台坐标系的原点op，opxp轴位于云台的倾转轴上，正方向指向右侧，opyp位于滚转轴上，正方向指向云台的后方，opzp轴垂直于xpopyp平面并指向下方；

设定假设相机光心、云台中心和无人机机体重心三者重合，则在视觉跟踪阶段，只控制俯仰角θ和偏航角，而不用考虑ψ滚转角，因为云台滚转只影响图像的方向，而不会影响图像的位置；

2)、计算无人机在地面坐标系中的位置坐标，假设无人机当前的位置坐标为(xa、ya、za)：

2.1)、根据姿态角，并结合无人机上气压计上的显示值可以计算出无人机的当前飞行高度h，即za＝h；

2.2)、无人机上设置有gps定位装置，可以直接读出测量值，以无人机起飞点为地面坐标系的原点，测得原点处的经度值lo0、纬度值la0，无人机实际执行追踪命令时当前的经度值为lo1、纬度值为la1，则原点与无人机当前位置的经度偏差为(lo1-lo0)、纬度偏差为(la1-la0)；设定在同一经度上纬度偏差1度所对应的距离是定值，为111km；纬度偏差1分所对应的距离为1.85km，纬度偏差1秒所对应的距离为31.8m；

则xa＝111(la1-la0)；

同一纬度上经度偏差1度所对应的距离随纬度增高逐渐减小，可按以下公式计算：经度偏差1度所对应的距离＝111.413coslai-0.094cos(3lai)；lai为纬度值；

则ya＝(lo1-lo0)[111.413cosla1-0.094cos(3la1)]；

得出当前无人机在地面坐标系的位置(xa、ya、za)；

3)、计算追踪目标在地面坐标系中的位置坐标，

3.1)、当姿态角为(0、90、0)，即云台朝向垂直向下，无人机位于追踪目标物正上方：

3.2)、当姿态角为：设定在地面坐标系中目标物与无人机的坐标偏差量为(a、b、c)，

则

c＝-h；

则目标物在地面坐标系中的坐标为

4)、控制器根据目标物的位置精确控制无人机进行追踪目标物：

无人机在飞行过程中，云台目标追踪的输出量为云台的姿态角和被追踪目标的实际距离，测量距离存在偏差，控制器使用aruco图案识别算法，用相机内参矩阵进行像素偏差到实际距离偏差的转换，用dx表示x方向的实际偏差距离，px表示x方向的像素偏差值，dx/f为内参矩阵输出数据，h为无人机飞行高度，即有：dx＝px*dx*h/f；

用dy表示y方向的实际偏差距离，py表示y方向的像素偏差值，dy/f为内参矩阵输出数据，h为高度，即有dy＝py*dy*h/f；

控制器根据实际偏差距离dx、dy来控制无人机进行追踪目标物，dx、dy的方向性由px、py决定，形成闭环参数，完成对无人机的精准控制，实现精确视觉追踪。

与现有技术相比，本方案具有如下有益效果：

无人机在追踪目标物过程中，根据无人机gps，得出无人机当前位置，根据云台姿态角和无人机当前位置计算出目标物当前位置，控制器根据目标物当前位置精确控制无人机实现精确视觉追踪。使用gps+图像识别的组合方式来精确定位，实现无人机实现精确视觉追踪，控制精度高。

附图说明

图1为本发明所述无人机视觉追踪实现方法的流程图。

具体实施方式

为使本发明的目的和技术方案更加清楚，下面结合本发明实施例对本发明的技术方案进行清楚，完整的描述。

实施例一：

如图1中的方法一所示，一种无人机视觉追踪实现方法，包括如下步骤：

1)、进行无人机摄像机标定、获取摄像机内参矩阵；

2)、无人机起飞，摄像机拍摄图像，无人机控制器获取图像数据并通过无线通讯模块实时发送给地面站控制系统；

3)、地面站控制系统实时检测接收的图像数据，并判断被追踪目标是否出现当前图像中，即摄像机视野中：

3.1)、如果否，则地面站控制系统遥控摄像机的云台，再次拍摄图像，并传递给地面站控制系统检测，所述云台为三轴云台；

3.2)、如果是，则地面站控制系统在当前图像中框选出被追踪目标，并将框选出的被追踪目标数据传递给无人机；

4)、无人机控制器对框选出的被追踪目标数据进行校验，判断框选出的被追踪目标数据是否有效：

4.1)、如果无效，则将校验失败的结果传递给地面站控制系统，传递给地面站控制系统重新在当前图像中框选被追踪目标；

4.2)、如果有效，则无人机进行被追踪目标识别，并判断是否成功识别到框选出的被追踪目标；

4.2.1)、如果失败，则将识别失败的结果传递给地面站控制系统，传递给地面站控制系统重新在当前图像中框选被追踪目标；

4.2.2)、如果成功，则无人机控制器根据图像识别输出的像素偏差控制云台对准待追踪目标；

5)、判断追踪目标是否位于图像中心：

5.1)如果否，则无人机控制器重新根据当前图像识别输出的像素偏差控制云台对准待追踪目标；

5.2)如果是，则获取此时云台的姿态角姿态角其中，ψ为滚转角，θ为俯仰角，为偏航角，无人机控制器根据姿态角控制无人机继续飞行；根据无人机上设置的加速度计、陀螺仪、磁传感器可以直接得出云台姿态角；

6)、判断无人机是否飞行至追踪目标正上方：

6.1)如果否、则控制器继续根据此时云台的姿态角控制无人机继续飞行；

6.2)如果是，则无人机达到机视觉追踪的目的。

在本实施例中，获取相机的内参矩阵的步骤如下；

1.1)、建立相机坐标系、成像平面坐标系、像素坐标系；

相机坐标系：以相机光心oc作为原点，ocxc轴平行与成像平面的水平方向，从相机的后方看，指向相机的右方，ocyc轴平行于成像平面竖直方向，指向相机下方，光轴oczc垂直与xcocyc平面；

像素坐标系：为一个二维直角坐标系，以像素为单位，以图像的左上点为原点o，ou轴与图像宽度方向平行，沿图像上顶边指向右侧，ov轴与图像高度方向平行，沿图像左边界指向下方。

成像平面坐标系：为一个二维直角坐标系，以图像中心oi为原点，该点为相机光轴与成像平面的交点，oixi轴与ocxc轴平行，oiyi轴与ocyc轴平行，并且两者的正方向相同；

1.2)完成相机坐标系、成像平面坐标系、像素坐标系之间的转换，

1.2.1)、将相机坐标系转换到成像平面坐标系：

设点a(x，y，z)为相机坐标系空间中的一点，则a(x，y，f)点为a点在到图像平面上的投影，相机的焦距为f，得出：

x/f＝x/z，y/f＝y/z；

即，x＝fx/z，y＝fy/z；

上述变换关系用3*3的矩阵表示为：q＝mq，其中

得出透视投影变换矩阵为：

1.2.2)、成像平面坐标系转换到像素坐标系：

设定成像平面坐标的原点oi在以像素为单位的成像平面坐标中的坐标为(u0，v0)；设每个像素的物理尺寸大小为dx*dy(mm)，dx≠dy；

设定图像平面上某点在成像平面坐标系中的坐标为(x，y)，在像素坐标系中的坐标为(u,v)，则二者满足如下关系：

u＝x/dx+u0；v＝y/dy+v0；

用齐次坐标与矩阵形式表示为：

将等式两边都乘以z可得：

将相机坐标系中的(1)式代入上式可得：

则获得相机的内参矩阵：

在本实施例中，所述步骤5.1)中，建立成像坐标系：为一个二维直角坐标系，以图像中心oi为原点，该点由相机光轴与图像平面的交点，oixi轴水平，向右为正方向，oiyi轴垂直，向上为正方向。

为了提高控制云台的柔和性使用角速率控制方法控制，设定px为此刻所拍摄图像在oixi方向的像素偏差值，py为在oiyi方向的像素偏差值，控制器以kpx、hpy作为进行云台闭环控制的输入量，其中，k为oixi方向云台追踪目标的速度系数，h为为oiyi方向云台追踪目标的速度系数，kpx用于云台的偏航角控制，hpy用于云台的俯仰角控制。

k、h值越大追踪速度越快，但是如果太大有可能出现振荡。因此需要k、h值需要实际的应用场景调节。kpx用于云台的偏航角控制，hpy用于云台的俯仰角控制。kpx、hpy是以o为坐标原点，所以能够产生一个闭环控制量，控制云台对准待追踪目标。

在本实施例中，所述步骤5.2)中，无人机获取此时云台的姿态角其中，ψ为滚转角，θ为俯仰角，为偏航角，根据目标物和无人机在地面坐标系中的坐标偏差作为输入量控制无人机飞行的步骤如下：

1)、建立地面坐标系、机体坐标系、云台坐标系、相机坐标系、像素坐标系和成像坐标系：

地面坐标系：原点og为旋翼无人机的起飞点，ogxg轴在地平面内指向地球北极或旋翼无人机的前飞方向，ogzg轴垂直与地平面朝下，ogyg轴垂直于xgogzg平面，正方指向右侧；

机体坐标系：ob为旋翼无人机中心，obxb轴指向机体正前方，obyb轴指向机体右侧，obzb轴垂直与xbobyb平面并指向机体下方；

云台坐标系：定义云台三个旋转轴的交点为云台坐标系的原点op，opxp轴位于云台的倾转轴上，正方向指向右侧，opyp位于滚转轴上，正方向指向云台的后方，opzp轴垂直于xpopyp平面并指向下方；

设定假设相机光心、云台中心和无人机机体重心三者重合，则在视觉跟踪阶段，只控制俯仰角θ和偏航角而不用考虑ψ滚转角，因为云台滚转只影响图像的方向，而不会影响图像的位置；

2)、计算无人机在地面坐标系中的位置坐标，假设无人机当前的位置坐标为(xa、ya、za)：

2.1)、根据姿态角并结合无人机上气压计上的显示值可以计算出无人机的当前飞行高度h，即za＝h；

2.2)、无人机上设置有gps定位装置，可以直接读出测量值，以无人机起飞点为地面坐标系的原点，测得原点处的经度值lo0、纬度值la0，无人机实际执行追踪命令时当前的经度值为lo1、纬度值为la1，则原点与无人机当前位置的经度偏差为(lo1-lo0)、纬度偏差为(la1-la0)；设定在同一经度上纬度偏差1度所对应的距离是定值，为111km；纬度偏差1分所对应的距离为1.85km，纬度偏差1秒所对应的距离为31.8m；

则xa＝111(la1-la0)；

同一纬度上经度偏差1度所对应的距离随纬度增高逐渐减小，可按以下公式计算：经度偏差1度所对应的距离＝111.413coslai-0.094cos(3lai)；lai为纬度值；

则ya＝(lo1-lo0)[111.413cosla1-0.094cos(3la1)]；

得出当前无人机在地面坐标系的位置(xa、ya、za)；

3)、计算追踪目标在地面坐标系中的位置坐标，

3.1)、当姿态角为(0、90、0)，即云台朝向垂直向下，无人机位于追踪目标物正上方：

3.2)、当姿态角为：设定在地面坐标系中目标物与无人机的坐标偏差量为(a、b、c)，

则

c＝-h；

则目标物在地面坐标系中的坐标为

4)、控制器根据目标物的位置精确控制无人机进行追踪目标物：

无人机在飞行过程中，云台目标追踪的输出量为云台的姿态角和被追踪目标的实际距离，测量距离存在偏差，控制器使用aruco图案识别算法，用相机内参矩阵进行像素偏差到实际距离偏差的转换，用dx表示x方向的实际偏差距离，px表示x方向的像素偏差值，dx/f为内参矩阵输出数据，h为无人机飞行高度，即有：dx＝px*dx*h/f；

用dy表示y方向的实际偏差距离，py表示y方向的像素偏差值，dy/f为内参矩阵输出数据，h为高度，即有dy＝py*dy*h/f；

控制器根据实际偏差距离dx、dy来控制无人机进行追踪目标物，dx、dy的方向性由px、py决定，形成闭环参数，完成对无人机的精准控制，实现精确视觉追踪。

实施例二：

如图1中的方法二所示，在本实施例中，所述使用云台的姿态角作为输入量控制无人机的方法如下：计算无人机偏航角与云台偏航角差，以及无人机俯仰角与云台俯仰角差，判断偏航角差是否等于0度：

1)、如果不等于0，则控制无人机改变自身姿态，向减小偏航角差的方向旋转，并再次判断；

2)如果等于0，则判断俯仰角差是否等于90度：

2.1)、如果不等于90度，则控制无人机改变自身姿态，向减小俯仰角差的方向飞行并再次判断；

2.2)、如果等于90度，则无人机位于追踪目标正上方，然后判断追踪任务是否结束：

2.2.1)、如果否，则使用云台的当前姿态角作为输入量控制无人机继续飞行；

2.2.2)、如果是，无人机达到机视觉追踪的目的。

其他部分与实施例一中相同。

本发明中未做特别说明的均为现有技术或者通过现有技术即可实现，而且本发明中所述具体实施案例仅为本发明的较佳实施案例而已，并非用来限定本发明的实施范围。即凡依本发明申请专利范围的内容所作的等效变化与修饰，都应作为本发明的技术范畴。