CN109753076A - 一种无人机视觉追踪实现方法 - Google Patents

Abstract

本发明公开了一种无人机视觉追踪实现方法，包括1)、进行无人机摄像机标定、获取摄像机内参矩阵；2)、无人机起飞，摄像机拍摄图像，并发送给地面站控制***；3)、地面站控制***遥控摄像机的云台，被追踪目标位于图像中心；4)、获取此时云台的姿态角无人机控制器根据姿态角控制无人机继续飞行；5)、判断无人机是否飞行至追踪目标正上方，则无人机达到机视觉追踪的目的。无人机在追踪目标物过程中，根据无人机GPS，得出无人机当前位置，根据云台姿态角

Description

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

技术领域

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

背景技术

随着无人机技术越来越成熟，无人机的应用领域也越来越广泛，近两年除消费级无人机火爆以外，无人机在其他很多行业也得到广泛发展，如物流无人机、航拍无人机、侦查无人机等，无人机在执行任务过程中需要跟踪目标物，目前无人机定位采用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)、建立相机坐标系、成像平面坐标系、像素坐标系；

相机坐标系：以相机光心O_c作为原点，O_cX_c轴平行与成像平面的水平方向，从相机的后方看，指向相机的右方，O_cY_c轴平行于成像平面竖直方向，指向相机下方，光轴O_cZ_c垂直与X_cO_cY_c平面；

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

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

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)、成像平面坐标系转换到像素坐标系：

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

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

u＝x/dx+u₀；v＝y/dy+v₀；

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

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

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

则获得相机的内参矩阵：

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

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

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

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

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

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

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

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

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

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

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

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

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

机体坐标系：O_b为旋翼无人机中心，O_bX_b轴指向机体正前方，O_bY_b轴指向机体右侧，O_bZ_b轴垂直与X_bO_bY_b平面并指向机体下方；

云台坐标系：定义云台三个旋转轴的交点为云台坐标系的原点O_p，O_pX_p轴位于云台的倾转轴上，正方向指向右侧，O_pY_p位于滚转轴上，正方向指向云台的后方，O_pZ_p轴垂直于X_pO_pY_p平面并指向下方；

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

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

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

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

则Xa＝111(La₁-La₀)；

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

则Ya＝(Lo₁-Lo₀)[111.413cos La₁-0.094cos(3La₁)]；

得出当前无人机在地面坐标系的位置(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)、建立相机坐标系、成像平面坐标系、像素坐标系；

相机坐标系：以相机光心O_c作为原点，O_cX_c轴平行与成像平面的水平方向，从相机的后方看，指向相机的右方，O_cY_c轴平行于成像平面竖直方向，指向相机下方，光轴O_cZ_c垂直与X_cO_cY_c平面；

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

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

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)、成像平面坐标系转换到像素坐标系：

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

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

u＝x/dx+u₀；v＝y/dy+v₀；

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

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

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

则获得相机的内参矩阵：

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

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

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

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

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

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

机体坐标系：O_b为旋翼无人机中心，O_bX_b轴指向机体正前方，O_bY_b轴指向机体右侧，O_bZ_b轴垂直与X_bO_bY_b平面并指向机体下方；

云台坐标系：定义云台三个旋转轴的交点为云台坐标系的原点O_p，O_pX_p轴位于云台的倾转轴上，正方向指向右侧，O_pY_p位于滚转轴上，正方向指向云台的后方，O_pZ_p轴垂直于X_pO_pY_p平面并指向下方；

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

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

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

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

则Xa＝111(La₁-La₀)；

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

则Ya＝(Lo₁-Lo₀)[111.413cos La₁-0.094cos(3La₁)]；

得出当前无人机在地面坐标系的位置(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)、如果是，无人机达到机视觉追踪的目的。

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

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

Claims (6)

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)如果是，则无人机达到机视觉追踪的目的。

2.根据权利要求1所述的无人机视觉追踪实现方法，其特征在于，所述步骤1)中获取相机的内参矩阵的步骤如下；

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

相机坐标系：以相机光心O_c作为原点，O_cX_c轴平行与成像平面的水平方向，从相机的后方看，指向相机的右方，O_cY_c轴平行于成像平面竖直方向，指向相机下方，光轴O_cZ_c垂直与X_cO_cY_c平面；

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

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

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)、成像平面坐标系转换到像素坐标系：

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

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

u＝x/dx+u₀；v＝y/dy+v₀；

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

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

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

则获得相机的内参矩阵：

3.根据权利要求1或2所述的无人机视觉追踪实现方法，其特征在于，所述步骤5.1)中，建立成像坐标系：为一个二维直角坐标系，以图像中心O_i为原点，该点由相机光轴与图像平面的交点，O_iX_i轴水平，向右为正方向，O_iY_i轴垂直，向上为正方向；

设定p_x为此刻所拍摄图像在O_iX_i方向的像素偏差值，p_y为在O_iY_i方向的像素偏差值，控制器以kp_x、hp_y作为进行云台闭环控制的输入量，其中，k为O_iX_i方向云台追踪目标的速度系数，h为为O_iY_i方向云台追踪目标的速度系数，kp_x用于云台的偏航角控制，hp_y用于云台的俯仰角控制。

4.根据权利要求3所述的无人机视觉追踪实现方法，其特征在于，所述步骤5.2)中，无人机获取此时云台的姿态角其中，ψ为滚转角，θ为俯仰角， 为偏航角，控制器根据姿态角控制无人机飞行追踪目标物包括两种策略：

一种为使用云台的姿态角作为输入量进行控制；

另一种为根据目标物和无人机在地面坐标系中的坐标偏差作为输入量控制无人机飞行。

5.根据权利要求4所述的无人机视觉追踪实现方法，其特征在于，所述使用云 台的姿态角作为输入量控制无人机的方法如下：

计算无人机偏航角与云台偏航角差，以及无人机俯仰角与云台俯仰角差，判断偏航角差是否等于0度：

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

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

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

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

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

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

6.根据权利要求5所述的无人机视觉追踪实现方法，其特征在于，所述根据目标物和无人机在地面坐标系中的坐标偏差作为输入量控制无人机飞行的步骤如下：

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

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

机体坐标系：O_b为旋翼无人机中心，O_bX_b轴指向机体正前方，O_bY_b轴指向机体右侧，O_bZ_b轴垂直与X_bO_bY_b平面并指向机体下方；

云台坐标系：定义云台三个旋转轴的交点为云台坐标系的原点O_p，O_pX_p轴位于云台的倾转轴上，正方向指向右侧，O_pY_p位于滚转轴上，正方向指向云台的后方，O_pZ_p轴垂直于X_pO_pY_p平面并指向下方；

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

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

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

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

则Xa＝111(La₁-La₀)；

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

则Ya＝(Lo₁-Lo₀)[111.413cos La₁-0.094cos(3La₁)]；

得出当前无人机在地面坐标系的位置(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决定，形成闭环参数，完成对无人机的精准控制，实现精确视觉追踪。