一种基于导航信息的无人机自主回收目标预测方法与流程

本发明涉及无人机自主回收技术领域，特别是涉及一种基于导航信息的无人机自主回收目标预测方法。

背景技术：

当前对无人机进行自主回收(地面回收或者车载回收等)在民用领域是一个研究的热点和难点，而要实现安全的自主回收，准确实时的相对位姿估计是基础，特别是对于小型无人机(特别是高速运动的无人机)而言更是一个挑战。

目前常用的自主回收方法是基于合作目标的方法，通过检测和跟踪地面或者车上的合作目标来对无人机的相对位姿进行估计，然而小型无人机载荷有限，机载处理能力受到限制，常规的基于合作目标的方法很可能不能满足实时性要求，或者由于合作目标在图像中的位置变化过快导致位姿估计效果差。

因此，亟需研究处一种方法用来一定程度上解决无人机自主回收中合作目标预测的实时性和准确性的问题。

技术实现要素：

为克服现有技术存在的缺陷，针对合作目标为运动的情况下，本发明提供一种基于导航信息的无人机自主回收目标预测方法。合作目标运动意味着合作目标在世界坐标系中的位移分量会发生改变，即发生变化，因此首先要能得到的值。

在无人机自主回收的过程中，无人机能够实时提供位姿等导航信息，即和已知。本发明基于前一段时刻的无人机的位姿信息，利用相关的数值方法拟合合作目标的运动曲线，根据该曲线预估合作目标当前时刻的空间运动位置，再结合导航信息预测合作目标的成像点。具体实施步骤如下：

(1)定义坐标系

摄像机坐标系ocxcyczc：原点为摄像机的光心，ocxc和ocyc轴与图像的u,v轴平行，oczc轴为摄像即光轴，焦距为f，ocxc和ocyc方向的有效焦距分别为fx和fy；

无人机机体坐标系obxbybzb：ocyc轴沿机身对称轴线指向机头，ocxc垂直于无人机对称面指向机身右方，oczc轴满足右手法则；

成像平面坐标系：原点为图像的中心点，横坐标x和纵坐标y分别平行于图像所在的行和列；

图像坐标系：原点为图像的左上角，横坐标u和纵坐标v分别是图像所在的行和列，中心点(u0,v0)为主点坐标；

(2)合作标志运动意味着合作标志在世界坐标系中的位移分量会发生改变，即发生变化。设目标点p为合作目标的中心点，如图2所示，考虑针孔摄像机模型，其中oc点为光心，oczc轴为光轴，f为焦距。目标点p相对于摄像机光心oc的距离在摄像机坐标系的投影(xc,yc,zc)可以表示为:

其中为目标点p在世界坐标系中的投影，为无人机在世界坐标系中的投影，为无人机机体坐标系到摄像机坐标系的旋转矩阵，为无人机的姿态旋转矩阵。

假设目标点p在成像平面坐标系和图像坐标系的坐标分别为(x,y)和(u,v)，图像坐标系中的主点坐标为(u0,v0)，则目标点p在这两个坐标系下的投影有如下关系

(3)基于多项式拟合估计合作目标在下一时刻的空间位置。

假设无人机在前n个时刻的相对位置为(xc(tk-i),yc(tk-i),zc(tk-i))，其中i＝1,…n；结合合作目标相对位姿信息，可以获得前n个时刻的合作目标的空间位置其中i＝1,…n；

考虑在时间区间[tk-i,tk-1]内，合作目标的运动曲线可以用低阶多项式来拟合，即

利用式(3)可以获得多项式的系数(cx0,cx1,cx2,…cxn)(cy0,cy1,cy2,…cyn)(cz0,cz1,cz2,…czn)；因此当tk时刻时，可以根据得到的多项式系数，结合式(3)估计合作目标在该时刻的空间位置

(4)预测合作目标的成像位置。

根据tk时刻的导航信息和以及tk时刻估计的利用式(1)可以得到(xc(tk),yc(tk),zc(tk))，再根据公式(2)，可以预测合作目标的成像像素点(u(tk),v(tk))。

与现有技术相比，本发明能够产生以下技术效果：

考虑在位姿估计过程中，并不需要对整幅图像进行处理，而只需要对目标区域进行处理就可以，从而可以减少检测时间。同时无人机上集成了导航系统，能够实时提供导航信息，而目标成像几何原理是与导航信息有关的。因此本发明结合导航信息可以预测合作目标的成像区域，仅在该区域内进行目标检测跟踪以及相对位姿解算，从而提高合作目标位姿估计算法的实时性和准确性。

附图说明

图1是一种合作目标的示意图。

图2是摄像机成像原理示意图。

具体实施方式

下面对本发明的进行详细阐述，以使本发明的优点和特征能更易于被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。

针对合作目标为运动的情况下，本发明提供一种基于导航信息的无人机自主回收目标预测方法。合作目标运动意味着合作目标在世界坐标系中的位移分量会发生改变，即发生变化，因此首先要能得到的值。在无人机自主回收的过程中，无人机能够实时提供位姿等导航信息，本发明基于前一段时刻的无人机的位姿信息，利用相关的数值方法拟合合作目标的运动曲线，根据该曲线预估合作目标当前时刻的空间运动位置，再结合导航信息预测合作目标的成像点。

下面给出一具体实施例：

案例：假设摄像机的焦距为fx＝fy＝1000，分辨率为1280×720；当前tk时刻无人机在世界坐标系中的投影无人机的姿态旋转矩阵为单位矩阵，摄像机与无人机机体的旋转矩阵地面上的合作目标以2m/s的速度匀速运动，且前期估计的合作目标的空间位置序列其中i＝1,…5(这里以5个值为例，间隔dt＝0.2s)，分别为(9.2,8.9,0.1)，(9.3,9.3,0.15)，(9.15,9.5,0.2)，(9.25,9.8,0.05)，(9.5,10.2,0.12)。

需要预测当前tk时刻合作目标的成像位置。

采用本发明方法，步骤如下：

1)根据已知的合作目标空间位置序列，由于合作目标运动，因此首先根据公式(3)基于前期估计的合作目标的空间位置序列用多项式来拟合合作目标运动轨迹，即求取多项式系数，由于这里采用前5组数据，因此根据公式(3)可以获得3阶多项式系数为：

cx0＝-0.6,cx1＝6.9583,cx2＝-15.625,cx3＝10.4167

cy0＝-0.3,cy1＝5,cy2＝-8.75,cy3＝6.257

cz0＝-0.24,cz1＝3.35,cz2＝-10.625,cz3＝8.125

2)根据1)的结果和公式(3)预测当前tk时刻目标的空间位置

3)根据公式(1)获得目标在摄像机坐标系的投影(xc(tk),yc(tk),zc(tk))。

4)根据公式(2)预测tk时刻合作目标在图像中的成像位置(u(tk),v(tk))。

以上所述仅为本发明的优选的实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。