一种动态场景下基于闪烁光信号的定位定姿算法制造技术

一种动态场景下基于闪烁光信号的定位定姿算法。其步骤如下：一、图像信息与惯导数据采集；二、建立坐标系；三、星点提取；四、图像配准；五、光源识别；六、位姿解算。通过上述流程提出了一种动态场景下基于闪烁光信号的定位定姿算法，通过星点提取获得光源质心位置，再通过光源识别获取光源三维位置，从而进行位姿解算。该算法充分利用惯性导航数据与光源的闪烁特性，能够避免与光源之间需要通信的问题，保证算法的鲁棒性，并在用户姿态变化的条件下实现较高精度的定位定姿。现较高精度的定位定姿。现较高精度的定位定姿。

【技术实现步骤摘要】
一种动态场景下基于闪烁光信号的定位定姿算法


[0001]本专利技术提供一种基于闪烁光信号的动态定位算法，它涉及运用星点提取和图像配准，在用户自身运动条件下进行定位定姿的方法，属于导航定位定姿


技术介绍

[0002]目前应用范围最广泛的导航定位系统是全球导航卫星系统(Global Navigation Satellite System，GNSS)，它能够全天时、高精度的进行定位授时，在众多民用、军用领域发挥了重大作用。但由于其无线电信号能量较弱，在地下、室内等封闭环境中，信号易被干扰，定位效果较差。
[0003]不依靠GNSS的导航方式主要有惯性导航(Inertial Navigation System，INS)、天文导航(Celestial Navigation System)、激光导航(Laser Navigation System)、视觉导航(Vision Navigation)等。其中，视觉导航由于其成本较低、精度高的特点，在室内导航、自动驾驶等领域发挥了重要作用。视觉导航主要由摄像机与数据处理单元组成，通过对特征点拍摄，恢复多幅图像的间的相对位姿，并利用特征点的二维投影信息恢复相机的三维位姿。一方面，视觉导航不仅能给出用户的位置信息，还能给出用户的姿态，这对于飞机、导弹等载体有着重大意义；另一方面，视觉导航的利用摄像机拍摄连续图片，位姿的更新速率快。此外，视觉导航与惯性导航结合也是重要的应用方向，惯性导航的加入解决了纯视觉导航对于快速旋转场景无法处理的问题，并减轻了对于场景纹理特征、光照条件的依赖性，而视觉导航也可以补偿和修正陀螺仪的漂移，保证位置与姿态的精度。因此，视觉导航与惯性导航所构成的组合导航系统也逐渐投入应用。
[0004]选取特征点是视觉导航的重要步骤，一般可以选取场景的固定特征信息作为特征点，也可以提前布置人工光信标。光导航是视觉导航领域的重要组成部分，在地下救援、室内导航等领域有着重大的应用前景。现有利用光信号进行定位的研究多在室内定位领域，并大多只适用于准静态场景或低速场景，且光信标所产生的的光信号恒定不变，在定位过程中需要与光源进行通信，或者测量光信号强度来估计距离，这增加了定位系统的复杂度，也使得定位精度的提升变得困难。如何在用户自身运动的条件下，只利用光信号实现定位与定姿，并保证系统可靠性，是扩大光导航应用范围的重点与难点。

技术实现思路

[0005](一)专利技术目的
[0006]本专利技术利用人为调制的闪烁光信号在场景中能较为容易识别的特性，预先设置人造光源的闪烁编码，根据单目相机的投影成像模型，结合惯性导航与姿态运动学的理论，提出一种动态场景下基于闪烁光信号的定位定姿算法。该算法充分利用惯性导航数据与光源的闪烁特性，能够避免与光源之间需要通信的问题，并在用户姿态变化的条件下实现较高精度的定位定姿。
[0007](二)技术方案
[0008]本专利技术涉及一种动态场景下基于闪烁光信号的定位定姿算法，该算法需要一个定位演示平台作为硬件支持，其组成如下：相机1、计算机2、惯性导航元件3、人造光源4。参照附图1。计算机是数据处理器，其上与相机和惯导元件相连并读取图像信息与姿态变化数据。
[0009]本专利技术需要提前将多个人造光源布置固定好，并将光源的闪烁编码与三维位置预先存储至计算机中；将计算机、相机、惯导元件组成的接收机固定在用户身上，并保证相机拍摄与光源闪烁之间时钟同步。当定位开始时，光源开始工作，用户可以自由进行角速度不太大的姿态运动，由相机和惯导元件记录下图像与角速度数据，然后由计算机计算得到用户的位姿。
[0010]本专利技术所述的动态场景下基于闪烁光信号的定位定姿算法实施步骤如下：
[0011]步骤一：图像信息与惯导数据采集
[0012]用户自由运动，相机与惯导记录下实时图像信息与运动角速度，并传给计算机存储，持续一个光源的闪烁周期。
[0013]步骤二：建立坐标系
[0014]建立ENU坐标系为导航坐标系；用户坐标系与相机坐标系可以认为是统一的。建立如下的相机坐标系：X为偏航轴，Y为俯仰轴，Z为滚转轴；u,v为对应二维像素坐标轴。参照附图2。
[0015]假定相机由s坐标系转动到t坐标系按照偏航
‑
俯仰
‑
滚转的顺序，分别绕X轴、Y轴、Z轴旋转α、β、γ，则对应的姿态旋转矩阵为：
[0016][0017]对于连续的两帧，可以认为转动的角度极小，忽略二阶小量，可以近似认为:
[0018][0019]步骤三：星点提取
[0020]将已存储的每帧图像中的光点提取出来，使用星点提取算法。对星点提取的研究已有分块峰值点算法、局部熵算法等较为成熟的方法，此处可直接使用，不再赘述；通过星点提取将得到光源在各帧图像中的二维像素坐标。
[0021]步骤四：图像配准
[0022]由于用户处于运动状态，需要首先利用惯导数据将每一帧图像进行配准，即转变为准静态情形，才能进行光源的解码。
[0023]惯性导航的输入信息是相机在相机坐标系的运动，一般而言会输入六个量：偏航、俯仰、滚转的角速度以及xyz三个方向上的加速度。由于相机连续拍摄两帧之间的时间间隔极短，因此可以认为相机只发生了姿态的变化，且认为是匀速，位置的变化忽略不计。
[0024]设相机的内参数矩阵为光源在导航坐标系下的三维坐标为其中i代表光源序号；T
n
为相机在导航坐标系下的位置，R
k
为相机在拍摄第k帧图像时的绝对姿态，则根据相机的投影成像模型，有：
[0025][0026][0027]式中也叫深度因子；(u
i,k
,v
i,k
)为第i号光源在第k帧时的二维图像坐标，是其在相机系下的三维坐标。
[0028]设相机以[ω
x
,ω
y
,ω
z
]的角速度运动，该角速度以相机系为坐标基准，拍摄连续两帧之间的时间间隔为δt，则第k帧时的相机到第k+1帧时的姿态转换矩阵为：
[0029][0030]为了后续表达的整洁，将式(5)写为：
[0031][0032]于是在第k+1帧时有：
[0033][0034][0035]联立式(4)(7)(8)，得到：
[0036][0037]再将K的具体形式代入式(9)，得到：
[0038][0039]由式(4)注意到有代入(10)最终得到：
[0040][0041][0042][0043]光源二维像素坐标与相机的运动均为已知量，由此得到了第k帧的每个星点到第k+1帧的配准量，从而可以将拍摄得到的每一帧星点均配准至同一时刻的位置，使解码能够在准静态下进行。
[0044]步骤五：光源识别
[0045]在准静态情况下，同一星点的二维质心坐标只在某一特定小范围内变化。对这些范围求并集，得到通信域，然后判断配准后的质心坐标是否在选定区域内，得到该星点对应光源的闪烁编本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种动态场景下基于闪烁光信号的定位定姿算法，其特征在于：利用惯导数据对序列图像进行配准，将动态场景转变为准静态情形，再进行光源的解码。在准静态情况下，同一星点的二维质心坐标只在某一特定小范围内变化。对这些范围求并集，得到通信域，然后判断配准后的质心坐标是否在选定区域内，得到该星点对应光源的闪烁编码，最后与接收机内部储存的光源闪烁编码进行匹配，可以得到该光源在导航坐标系下的三维坐标。2.一种动态场...

【专利技术属性】
技术研发人员：陈培，周子钰，李彧辉，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：