[发明专利]一种基于吊舱姿态反馈的目标跟踪控制方法

摘要

本发明提供一种基于吊舱姿态反馈的目标跟踪控制方法。在已知目标检测成像结果的前提下，考虑到吊舱的2个姿态角(俯仰角和偏航角)的输出是已知的，同时吊舱和无人机是捷联的，因此可以利用吊舱的姿态信息来生成所需的角度输入，进而产生吊舱速度控制指令，从而使得目标保持在视场中心。通过该方法能够快速准确的控制吊舱的旋转，使得目标在视场中心。

主权项

1.一种基于吊舱姿态反馈的目标跟踪控制方法，其特征在于：包括以下步骤：(1)确定吊舱坐标系到摄像机坐标系的旋转的顺序为先偏航后俯仰最后再滚转；(2)定义坐标系和转换关系关于吊舱坐标系、摄像机坐标系：摄像机坐标系ocxcyczc原点为相机的光心，ocxc和ocyc轴与图像的u,v轴平行，oczc轴为相机光轴，焦距为f，ocxc和ocyc方向的有效焦距分别为fx和fy；吊舱坐标系opxpypzp的原点为吊舱安装轴的中心点，opxp和opyp轴分别为吊舱的右侧和正前方，opzp满足右手法则；为摄像机坐标系到吊舱坐标系的方向余弦矩阵，为t_j时刻吊舱坐标系到t_i时刻吊舱坐标系的方向余弦矩阵，其中i,j＝0,...k,i≠j，对应的欧拉角为分别表示偏航角，俯仰角和滚转角；为t_j时刻摄像机坐标系到t_i时刻摄像机坐标系的方向余弦矩阵，对应的欧拉角为(3)计算目标偏离视场中心的大小(Δu,Δv)其中(u0,v0)是图像的中心点，也叫主点坐标，(u1,v1)是检测到的目标在图像中的成像位置坐标；(4)计算在u，v方向上目标偏离的角度其中：Δψ表示偏离的偏航角；(5)根据欧拉角和旋转矩阵的转换关系，可以获得从t_k时刻到t_k+1时刻目标偏离角度的旋转矩阵(6)由于吊舱相对内置相机的旋转矩阵事先已经进行标定，即已知，而且上一时刻吊舱旋转的旋转矩阵已知，因此可以获得吊舱此时的旋转矩阵(7)根据旋转矩阵和欧拉角的转换关系和式(4)，得到吊舱的欧拉角输出由于吊舱为2自由度，默认滚转角为0，因此得到2自由度吊舱的输入角获得输入角后，即可生成吊舱的速度控制指令，从而实现目标的稳定跟踪。