本发明公开了一种基于地平线与偏振光的载体三维姿态获取方法。首先，利用载体的前视摄像头检测出地平线，根据地平线在图像中的位置以及相机参数可以推算出载体的横滚角和俯仰角；其次，根据偏振相机得到天空偏振信息图像，从偏振度图像中利用椭圆拟合方法提取出偏振中性点，根据偏振中性点在相机坐标系和地理坐标系的转换关系建立载体三维姿态方程；最后将由地平线导航得到的横滚角和俯仰角带入载体三维姿态方程，解算出载体航向角，从而得到载体的三维姿态。本发明方法具有全自主、无源性等优点。
1.一种基于地平线与偏振光的载体三维姿态获取方法，其特征在于，实现步骤如下：步骤(1)利用载体前视摄像机获取地平线信息，并计算出地平线在图像坐标系下的直线方程L，利用直线方程L、载体距地面高度H及相机参数估计出载体横滚角γ和俯仰角θ；
步骤(2)利用载体上视偏振相机获取天空偏振图像，计算出天空偏振度信息p和偏振方位角信息 并提取出偏振中性点；
步骤(3)通过相机标定计算出偏振中性点在相机坐标系下的方向向量 用 表示偏振中性点在地理坐标系下的方向向量，然后通过相机坐标系与机体坐标系的转换矩阵和机体坐标系和地理坐标系 之间的转换关系建立基于偏振中性点的载体三维姿态方程步骤(4)将由地平线导航估计出的横滚角γ和俯仰角θ带入基于偏振中性点的载体三维姿态方程，解出载体航向角ψ，得到载体三维姿态信息。
2.根据权利要求1所述的基于地平线与偏振光的载体三维姿态获取方法，其特征在于：所述步骤(1)中，用载体前视摄像机获取含地平线的图像，用边缘提取和霍夫变换提取出图像中的地平线，并计算出地平线在图像坐标系下的直线方程L：L:y＝kx+b
其中，x为图像坐标系横轴，y为图像坐标系纵轴，k为直线斜率，b为直线截距；
建立世界坐标系ow-xwywzw与相机坐标系oc-xcyczc，相机中心oc在世界坐标系下的坐标为rw＝[0 -h 0]T，也就是相机中心到平面owxwzw的距离为h，owxwzw平面的法向向量为nw＝[0
1 0]T，地平线上一点A在世界坐标系下的坐标为dw＝[xa 0 d]T，其中d也就是世界坐标系原点到地平线的距离，地平线在世界坐标系下的方向向量为lw＝[1 0 0]T，地平线在相机坐标系下的方向向量为lc，相机到点A的向量在世界坐标系下表示为aw，点A在相机坐标系下的坐标为ac，根据载体当前的姿态角可以得到由世界坐标系到相机坐标系的转换矩阵其中，γ是载体的横滚角，θ是载体的俯仰角；
相机坐标系到世界坐标系的转换矩阵为 其中 为 的逆矩阵，由 可以得到dw＝xw+rw， 因为dw和lw都与nw垂直，因此有：
通过相机成像模型、地平线在图像坐标系下的直线方程和上述两个等式，可以解算出载体的横滚角γ和俯仰角θ：γ＝arctan(-k)
其中，f为前视摄像机的焦距，由相机标定可得，d和h与载体离地面的高度有关；
载体距地面的高度H由高度计得出，R为地球平均半径，从而计算出世界坐标系原点到地平线的距离d和相机中心到平面owxwzw的距离h：至此，得到载体的横滚角γ和俯仰角θ。
3.根据权利要求1所述的基于地平线与偏振光的载体三维姿态获取方法，其特征在于：所述步骤(2)中，使用偏振相机获取一组天空偏振图像，其中包括光强信息分别为I1、I2和I3的三张天空偏振图像；用该组天空偏振图像的光强信息计算出相应的斯托克斯矢量S＝(I,Q,U,V)，根据斯托克斯矢量解出偏振度p和偏振方位角其中I表示天空光总量度，Q表示与参考方向平行的线偏振光，U表示与参考方向成45°角的线偏振光，V表示圆偏振光的强度，其比例很小，一般忽略不计；
由偏振度信息得到偏振度图像并进行图像预处理，平滑偏振度图像，根据偏振中性点处偏振度最低的特点，找到最大的两处低偏振度区域并对其进行椭圆拟合，对两个椭圆区域分别进行提取中心操作，提取的中心即为两个偏振中性点。
4.根据权利要求1所述的基于地平线与偏振光的载体三维姿态获取方法，其特征在于：所述步骤(3)中，任意选择一个偏振中性点，通过相机标定可得到该偏振中性点在相机坐标系下的方向向量其中， 和 分别是偏振中性点在相机坐标系下的方位角和高度角；
根据天文年历求出太阳在地理坐标系下的高度角 和方位角 由于偏振中性点位于太阳子午线或者反太阳子午线上，因此偏振中性点在地理坐标系下的方位角为或 通过光强信息判断所选偏振中性点位于太阳子午线上还是反太阳子午线上，并写出该偏振中性点在地理坐标系下的方向向量其中， 和 分别是偏振中性点在地理坐标系下的方位角和高度角，由偏振中性点在相机坐标系和地理坐标系下的转化关系，可以得到载体的三维姿态方程，如下式：
5.根据权利要求1所述的基于地平线与偏振光的载体三维姿态获取方法，其特征在于：所述步骤(4)中，将由地平线导航得到的载体横滚角γ和俯仰角θ带入由偏振中性点得到的载体三维姿态方程，解出载体航向角ψ，至此，载体的三个姿态角全部解出。
一种基于地平线与偏振光的载体三维姿态获取方法
技术领域
[0001] 本发明涉及一种基于地平线与偏振光的载体三维姿态获取方法，可用于载体在整个飞行过程中的三维姿态获取。
背景技术
[0002] 偏振光是由于太阳光在大气传输过程中受到大气粒子散射而形成的，天空中不同偏振态的偏振光形成了大气偏振模式。仿生偏振光导航就是依靠稳定的大气偏振模式的一种导航方法。偏振光导航有着无源无辐射等优点使其近年来被广泛研究。由于单一的偏振光导航只能获取载体的航向角，因此目前大多用偏振导航与惯性器件组合来获取载体三维姿态。已申请的中国专利“一种双模式仿生偏振/地磁辅助组合导航系统”，申请公开号CN105021188，使用仿生偏振导航、地磁导航与惯性导航结合来获取载体的三维姿态，其中载体的横滚角和俯仰角主要由惯性导航提供，因此可能会有累积误差。基于地平线的导航方法由于其简单、环境适应性强等优点被作为常用的导航方法之一，但是现有方法大多忽略了载体高度对地平线导航精度的影响。已发表的论文“基于直线模型的微型飞行器姿态角计算”，没有建立载体高度模型而是直接忽略高度对姿态角估计的影响，最后得到的导航模型会有相应的误差。地平线导航只能获取载体的横滚角和俯仰角，因此地平线导航多与基于其它标志物的视觉导航方法结合获取载体的三维姿态。已发表的论文“基于跑道的视觉导航信息分析”，利用地平线与跑道线的视觉信息建立了无人机在着陆时的三维姿态解算模型，但是这种方法仅适用于无人机自主着陆场合，不能在无人机自主飞行的时候实时得到载体三维姿态信息。
发明内容
[0003] 本发明要解决的技术问题是：克服现有技术的不足，提供一种基于地平线与偏振光的载体三维姿态获取方法。利用地平线导航得到载体的横滚角和俯仰角，利用偏振光导航建立基于偏振中性点的载体三维姿态方程，通过将由地平线导航得到载体的横滚角和俯仰角带入载体的三维姿态方程，解算出载体航向角，实现载体的三维姿态获取。
[0004] 本发明的技术方案是：一种基于地平线与偏振光的载体三维姿态获取方法，其实现步骤如下：
[0005] 步骤(1)利用载体前视摄像机获取地平线信息，并计算出地平线在图像坐标系下的直线方程L，利用直线方程L、载体距地面高度H及相机参数估计出载体横滚角γ和俯仰角θ；
[0006] 步骤(2)利用载体上视偏振相机获取天空偏振图像，计算出天空偏振度信息p和偏振方位角信息 并提取出偏振中性点；
[0007] 步骤(3)通过相机标定计算出偏振中性点在相机坐标系下的方向向量 用表示偏振中性点在地理坐标系下的方向向量，然后通过相机坐标系与机体坐标系的转换矩阵 和机体坐标系和地理坐标系 之间的转换关系建立基于偏振中性点的载体三维姿态方程
[0008] 步骤(4)将由地平线导航估计出的横滚角γ和俯仰角θ带入基于偏振中性点的载体三维姿态方程，解出载体航向角ψ，得到载体三维姿态信息。
[0009] 所述步骤(1)具体实现如下：
[0010] 用载体前视摄像机获取含地平线的图像，用边缘提取和霍夫变换提取出图像中的地平线，并计算出地平线在图像坐标系下的直线方程L：
[0011] L:y＝kx+b
[0012] 其中，x为图像坐标系横轴，y为图像坐标系纵轴，k为直线斜率，b为直线截距。
[0013] 建立世界坐标系ow-xwywzw与相机坐标系oc-xcyczc，相机中心oc在世界坐标系下的坐标为rw＝[0 -h 0]T，也就是相机中心到平面owxwzw的距离为h。owxwzw平面的法向向量为nw＝[0 1 0]T，地平线上一点A在世界坐标系下的坐标为dw＝[xa 0 d]T，其中d也就是世界坐标系原点到地平线的距离。地平线在世界坐标系下的方向向量为lw＝[1 0 0]T，地平线在相c w机坐标系下的方向向量为l ，相机到点A的向量在世界坐标系下表示为a ，点A在相机坐标系下的坐标为ac。根据载体当前的姿态角可以得到由世界坐标系到相机坐标系的转换矩阵[0014]
[0015] 其中，γ是载体的横滚角，θ是载体的俯仰角。
[0016] 相机坐标系到世界坐标系的转换矩阵为 其中 为 的逆矩阵，由 可以得到 dw＝xw+rw， 因为dw和lw都与nw垂直，因此有：
[0017]
[0018]
[0019] 其中，·表示点乘。
[0020] 通过相机成像模型、地平线在图像坐标系下的直线方程和上述两个等式，可以得到载体的横滚角γ和俯仰角θ的计算表达式：
[0021] γ＝arctan(-k)
[0022]
[0023] 其中，f为前视摄像机的焦距，由相机标定可得，世界坐标系原点到地平线的距离d和相机中心到平面owxwzw的距离h与载体离地面的高度有关。
[0024] 载体距地面的高度H由高度计得出，R为地球平均半径，从而计算出d和h：
[0025]
[0026]
[0027] 至此得到载体的横滚角γ和俯仰角θ。
[0028] 所述步骤(3)具体实现如下：
[0029] 使用载体的上视偏振相机获取一组天空偏振图像，用该组天空偏振图像的光强信息计算出相应的斯托克斯矢量S＝(I,Q,U,V)，根据斯托克斯矢量解出偏振度p和偏振方位角
[0030]
[0031] 其中I表示天空光总量度，Q表示与参考方向平行的线偏振光，U表示与参考方向成45°角的线偏振光，V表示圆偏振光的强度，其比例很小，一般忽略不计。
[0032] 由偏振度信息得到偏振度图像并进行图像预处理，平滑偏振度图像。根据偏振中性点处偏振度最低的特点，找到最大的两处低偏振度区域并对其进行椭圆拟合，对两个椭圆区域分别进行提取中心操作，提取的中心即为两个偏振中性点。
[0033] 所述步骤(4)具体实现如下：
[0034] 任意选择一个偏振中性点，通过相机标定可得到该偏振中性点在相机坐标系下的方向向量
[0035]
[0036] 其中， 和 分别是偏振中性点在相机坐标系下偏振中性点的方位角和高度角。根据天文年历求出太阳在地理坐标系下的高度角 和方位角 由于偏振中性点位于太阳子午线或者反太阳子午线上，因此偏振中性点在地理坐标系下的方位角为或 通过光强信息判断所选偏振中性点位于太阳子午线上还是反太阳子午线上，并写出该偏振中性点在地理坐标系下的方向向量
[0037]
[0038] 其中， 和 分别是偏振中性点在相机坐标系下的方位角和高度角。由偏振中性点在相机坐标系和地理坐标系下的转化关系，可以得到载体的三维姿态方程，如下式：
[0039]
[0040] 所述步骤(5)具体实现如下：
[0041] 将由(2)得到的载体横滚角γ和俯仰角θ带入由偏振中性点得到的载体三维姿态方程，解出载体航向角ψ，至此，载体的三个姿态角全部解出。
[0042] 本发明与现有技术相比的优点在于：
[0043] (1)与现有的偏振辅助惯性组合导航方法相比，不依赖其它惯性器件，因此载体的三维姿态角都没有随时间累积的误差。
[0044] (2)与现有的基于地平线的姿态测量方法相比，不仅可以获取载体的横滚角和俯仰角信息，还可以获取载体十分重要的航向角信息。与现有的基于地平线和标志物组合的姿态测量方法相比，不需要其他标志物就可以获取载体的三维姿态，可以适用于无任何标志点的单一环境，场景适用范围大大提高，并具有高度自主性。
附图说明
[0045] 图1为本发明一种基于地平线与偏振光的载体三维姿态获取方法的流程图；
[0046] 图2为前视摄像机坐标系与世界坐标系示意图；
[0047] 图3为载体高度示意图。
具体实施方式
[0048] 下面结合附图以及具体实施方式进一步说明本发明。
[0049] 如图1所示，本发明一种基于地平线与偏振光的载体三维姿态获取方法的具体实现步骤如下：
[0050] 步骤(1)利用载体前视摄像机获取地平线信息，并计算出地平线在图像坐标系下的直线方程L，利用直线方程L、载体距地面高度H及相机参数估计出载体横滚角γ和俯仰角θ；
[0051] 步骤(2)利用载体上视偏振相机获取一组天空偏振图像，计算出天空偏振度信息p和偏振方位角信息 并提取出天空偏振图像中的偏振中性点；
[0052] 步骤(3)通过相机标定计算出偏振中性点相机坐标系下的方向向量 用 表示偏振中性点在地理坐标系下的方向向量，然后通过相机坐标系与机体坐标系的转换矩阵和机体坐标系和地理坐标系 之间的转换关系建立基于偏振中性点的载体三维姿态方程
[0053] 步骤(4)将由地平线导航估计出的横滚角γ和俯仰角θ带入基于偏振中性点的载体三维姿态方程，解出载体航向角ψ，得到载体三维姿态信息。
[0054] 所述步骤(1)具体实现如下：
[0055] 用载体前视摄像机获取含地平线的图像，用边缘提取和霍夫变换提取出图像中的地平线，并计算出地平线在图像坐标系下的直线方程L：
[0056] L:y＝kx+b
[0057] 其中，x为图像坐标系横轴，y为图像坐标系纵轴，k为直线斜率，b为直线截距。
[0058] 建立世界坐标系ow-xwywzw与相机坐标系oc-xcyczc如图2所示，相机中心oc在世界坐w T w w w w w w标系下的坐标为r＝[0 -h 0] ，也就是相机中心到平面oxz的距离为h。ox z平面的法向向量为nw＝[0 1 0]T，地平线上一点A在世界坐标系下的坐标为dw＝[xa 0 d]T，其中d也就是世界坐标系原点到地平线的距离。地平线在世界坐标系下的方向向量为lw＝[1 0 0]T，地平线在相机坐标系下的方向向量为lc，相机到点A的向量在世界坐标系下表示为aw，点A在相机c
坐标系下的坐标为a。根据载体当前的姿态角可以得到由世界坐标系到相机坐标系的转换矩阵
[0059]
[0060] 其中，γ是载体的横滚角，θ是载体的俯仰角。
[0061] 相机坐标系到世界坐标系的转换矩阵为 其中 为 的逆矩阵，由 可w w w w w w以得到 d＝x+r， 因为d 和l都与n垂直，因此有：
[0062]
[0063]
[0064] 其中，·表示点乘。
[0065] 通过相机成像模型、地平线在图像坐标系下的直线方程和上述两个等式，可以得到载体的横滚角γ和俯仰角θ的计算表达式：
[0066] γ＝arctan(-k)
[0067]
[0068] 其中，f为前视摄像机的焦距，由相机标定可得，d和h与载体离地面的高度有关，如图3所示。
[0069] 载体距地面的高度H由高度计得出，从而计算出d和h：
[0070]
[0071]
[0072] 先根据地平线直线方程的斜率k结算出载体横滚角γ，再将相机焦距f、载体横滚角γ、计算出的d和h代入俯仰角θ的表达式中解算出载体俯仰角，至此得到载体的横滚角γ和俯仰角θ。
[0073] 所述步骤(2)具体实现如下：
[0074] 使用载体的上视偏振相机获取一组天空偏振图像，其中偏振片的方向相对于参考方向分别为0°、60°和120°，由此得到光强信息分别为I1、I2和I3的三张天空偏振图像；用该组天空偏振图像的光强信息计算出相应的斯托克斯矢量S＝(I,Q,U,V)，根据斯托克斯矢量解出偏振度p和偏振方位角
[0075]
[0076] 其中I表示天空光总量度，Q表示与参考方向平行的线偏振光，U表示与参考方向成45°角的线偏振光，V表示圆偏振光的强度，其比例很小，一般忽略不计。
[0077] 由偏振度信息得到偏振度图像并进行图像预处理，平滑偏振度图像。根据偏振中性点处偏振度最低的特点，找到最大的两处低偏振度区域并对其进行椭圆拟合，对两个椭圆区域分别进行提取中心操作，提取的中心即为两个偏振中性点。
[0078] 所述步骤(3)具体实现如下：
[0079] 任意选择一个偏振中性点，通过相机标定可得到该偏振中性点在相机坐标系下的方向向量
[0080]
[0081] 其中， 和 分别是偏振中性点在相机坐标系下方位角和高度角。根据天文年历求出太阳在地理坐标系下的高度角 和方位角 由于偏振中性点位于太阳子午线或者反太阳子午线上，因此偏振中性点在地理坐标系下的方位角为 或通过光强信息判断所选偏振中性点位于太阳子午线上还是反太阳子午线
上，并写出该偏振中性点在地理坐标系下的方向向量
[0082]
[0083] 其中， 和 分别是偏振中性点在相机坐标系下偏振中性点的方位角和高度角。由偏振中性点在相机坐标系和地理坐标系下的转化关系，可以得到载体的三维姿态方程，如下式：
[0084]
[0085] 假设安装时使得相机坐标系与机体坐标系一致，即 可以用载体的三个姿态角表示：
[0086]
[0087] 将 和 这两个转化矩阵代入 则载体的三维姿态方程可表示为：
[0088]
[0089] 所述步骤(4)具体实现如下：
[0090] 将由步骤(1)得到的载体横滚角γ和俯仰角θ带入由偏振中性点得到的载体三维姿态方程，解出载体航向角ψ，至此，载体的三个姿态角全部解出。
