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

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


背景技术：

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


技术实现要素：

5.(一)发明目的
6.本发明利用人为调制的闪烁光信号在场景中能较为容易识别的特性，预先设置人造光源的闪烁编码，根据单目相机的投影成像模型，结合惯性导航与姿态运动学的理论，提出一种动态场景下基于闪烁光信号的定位定姿算法。该算法充分利用惯性导航数据与光源的闪烁特性，能够避免与光源之间需要通信的问题，并在用户姿态变化的条件下实现较高精度的定位定姿。
7.(二)技术方案
8.本发明涉及一种动态场景下基于闪烁光信号的定位定姿算法，该算法需要一个定位演示平台作为硬件支持，其组成如下：相机1、计算机2、惯性导航元件3、人造光源4。参照附图1。计算机是数据处理器，其上与相机和惯导元件相连并读取图像信息与姿态变化数据。
9.本发明需要提前将多个人造光源布置固定好，并将光源的闪烁编码与三维位置预先存储至计算机中；将计算机、相机、惯导元件组成的接收机固定在用户身上，并保证相机拍摄与光源闪烁之间时钟同步。当定位开始时，光源开始工作，用户可以自由进行角速度不太大的姿态运动，由相机和惯导元件记录下图像与角速度数据，然后由计算机计算得到用户的位姿。
10.本发明所述的动态场景下基于闪烁光信号的定位定姿算法实施步骤如下：
11.步骤一：图像信息与惯导数据采集
12.用户自由运动，相机与惯导记录下实时图像信息与运动角速度，并传给计算机存储，持续一个光源的闪烁周期。
13.步骤二：建立坐标系
14.建立enu坐标系为导航坐标系；用户坐标系与相机坐标系可以认为是统一的。建立如下的相机坐标系：x为偏航轴，y为俯仰轴，z为滚转轴；u,v为对应二维像素坐标轴。参照附图2。
15.假定相机由s坐标系转动到t坐标系按照偏航-俯仰-滚转的顺序，分别绕x轴、y轴、z轴旋转α、β、γ，则对应的姿态旋转矩阵为：
[0016][0017]
对于连续的两帧，可以认为转动的角度极小，忽略二阶小量，可以近似认为:
[0018][0019]
步骤三：星点提取
[0020]
将已存储的每帧图像中的光点提取出来，使用星点提取算法。对星点提取的研究已有分块峰值点算法、局部熵算法等较为成熟的方法，此处可直接使用，不再赘述；通过星点提取将得到光源在各帧图像中的二维像素坐标。
[0021]
步骤四：图像配准
[0022]
由于用户处于运动状态，需要首先利用惯导数据将每一帧图像进行配准，即转变为准静态情形，才能进行光源的解码。
[0023]
惯性导航的输入信息是相机在相机坐标系的运动，一般而言会输入六个量：偏航、俯仰、滚转的角速度以及xyz三个方向上的加速度。由于相机连续拍摄两帧之间的时间间隔极短，因此可以认为相机只发生了姿态的变化，且认为是匀速，位置的变化忽略不计。
[0024]
设相机的内参数矩阵为光源在导航坐标系下的三维坐标为其中i代表光源序号；tn为相机在导航坐标系下的位置，rk为相机在拍摄第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]
在准静态情况下，同一星点的二维质心坐标只在某一特定小范围内变化。对这些范围求并集，得到通信域，然后判断配准后的质心坐标是否在选定区域内，得到该星点对应光源的闪烁编码，最后与接收机内部储存的光源闪烁编码进行匹配，可以得到该光源在导航坐标系下的三维坐标。
[0046]
判断二进制序列的相似性可以采用互相关的方法，求取两个离散序列的互相关函数：
[0047]
r(n)＝∑m[x(m)y(n+m)](13)
[0048]
式中：x和y代表离散序列，m表示整个可能的整数求和区域。
[0049]
将闪烁编码与存储的目标星编码进行互相关运算，通过比较互相关函数的最大值可以得到相似性最高的一组序列，该序列所对应的星点就是目标星代号，从而光源被成功识别，得到了光源对应三维坐标。
[0050]
步骤六：位姿解算
[0051]
利用牛顿-拉夫逊公式迭代求解，共有六个待求量，即位移tn＝[x
nynzn
]
t
，偏航角α，俯仰角β，滚转角γ，因此需要至少六个方程。为保证牛顿-拉夫逊迭代公式收敛至正确值，应寻求尽可能多的方程约束。就该算法而言，约束越多，方程对初值越不敏感。由于光源在不断闪烁，因此一帧图像极可能星点数目少，导致约束方程不足。为保证算法鲁棒性，需要利用多帧图像的星点信息以及惯导信息进行位姿求解。
[0052]
在这里使用三帧信息进行求解。假设要求解第k帧时的相机位姿，那么则利用第k-2帧至第k帧的信息进行求解。同样可以认为相机只有姿态上的变化。由式(1)，并为了表述的简洁，设第k帧时的相机姿态为：
[0053][0054]
首先利用第k帧的图像信息列方程。对某一星点，已知其对应光源的三维坐标，将其二维图像坐标归一化，即：
[0055][0056]
上式与(3)(4)联立得：
[0057][0058]
设将rk、tn具体形式代入(16)，并利用式子第三行消去前两行的深度因子，可以得到：
[0059][0060]
或写成：f
k,1
(θ)＝0(17)
[0061]
式中θ＝[α,β,γ,xn,yn,zn]
t
为待求列向量。类似的，对于其他星点可以求出f
k,2
(θ)＝0,f
k,3
(θ)＝0,
…
[0062]
对于第k-1帧、第k-2帧而言，利用惯导信息及式(5)，可以求出姿态转移矩阵于是有：
[0063][0064]rk-1
、r
k-2
矩阵的各项依旧只是α，β，γ的函数，于是类似于式(15)(16)的过程，可以列出：
[0065]fk-1，1
(θ)＝0，f
k-1，2
(θ)＝0，
…
(19)
[0066]fk-2，1
(θ)＝0，f
k-2，2
(θ)＝0，
…
[0067]
将式(17)(19)(20)组合，得到：
[0068][0069]
由此得到了牛顿-拉夫逊迭代的目标函数f(θ)。设f(θ)的雅克比矩阵为j，则迭代公式为：
[0070]
θ
k+1
＝θ
k-(j
t
j)-1jt
f(θk)(22)
[0071]
式中j代表j(θk)。由此得到了用户在导航坐标系下的位姿。
[0072]
综上所述，本发明的实施步骤流程如附图3所示。通过上述流程提出了一种动态场景下基于闪烁光信号的定位定姿算法，通过星点提取获得光源质心位置，再通过光源识别获取光源三维位置，从而进行位姿解算。该算法充分利用惯性导航数据与光源的闪烁特性，能够避免与光源之间需要通信的问题，保证算法的鲁棒性，并在用户姿态变化的条件下实现较高精度的定位定姿。
[0073]
(三)优点
[0074]
本发明提供的一种动态场景下基于闪烁光信号的定位定姿算法的优点在于：
[0075]
①
本发明提出的算法不需要光源与接收机之间进行额外的通信，只利用闪烁光信号即可定位。
[0076]
②
本发明提出的算法能够在用户自身运动的情形下实现定位。
附图说明
[0077]
图1是本发明所需的演示平台示意图。
[0078]
图2是本发明建立的坐标系。
[0079]
图3是本发明的算法流程步骤图。
[0080]
图1中，1相机、2计算机、3惯性导航元件、4人造光源。
具体实施方式
[0081]
下面将结合技术方案对本发明的具体实施过程做进一步的详细说明。
[0082]
本发明涉及一种动态场景下基于闪烁光信号的定位定姿算法，该算法需要一个定位演示平台作为硬件支持，其组成如下：相机1、计算机2、惯性导航元件3、人造光源4。参照附图1。计算机是数据处理器，其上与相机和惯导元件相连并读取图像信息与姿态变化数据。
[0083]
本发明需要提前将多个人造光源布置固定好，并将光源的闪烁编码与三维位置预先存储至计算机中；将计算机、相机、惯导元件组成的接收机固定在用户身上，并保证相机拍摄与光源闪烁之间时钟同步。当定位开始时，光源开始工作，用户可以自由进行角速度不太大的姿态运动，由相机和惯导元件记录下图像与角速度数据，然后由计算机计算得到用户的位姿。
[0084]
本发明所述的动态场景下基于闪烁光信号的定位定姿算法实施步骤如下：
[0085]
步骤一：图像信息与惯导数据采集
[0086]
用户自由运动，相机与惯导记录下实时图像信息与运动角速度，并传给计算机存储，持续一个光源的闪烁周期。
[0087]
步骤二：建立坐标系
[0088]
建立enu坐标系为导航坐标系；用户坐标系与相机坐标系可以认为是统一的。建立如下的相机坐标系：x为偏航轴，y为俯仰轴，z为滚转轴；u,v为对应二维像素坐标轴。参照附图2。
[0089]
假定相机由s坐标系转动到t坐标系按照偏航-俯仰-滚转的顺序，分别绕x轴、y轴、z轴旋转α、β、γ，则对应的姿态旋转矩阵为：
[0090][0091]
对于连续的两帧，可以认为转动的角度极小，忽略二阶小量，可以近似认为:
[0092][0093]
步骤三：星点提取
[0094]
将已存储的每帧图像中的光点提取出来，使用星点提取算法。对星点提取的研究已有分块峰值点算法、局部熵算法等较为成熟的方法，此处可直接使用，不再赘述；通过星点提取将得到光源在各帧图像中的二维像素坐标。
[0095]
步骤四：图像配准
[0096]
由于用户处于运动状态，需要首先利用惯导数据将每一帧图像进行配准，即转变为准静态情形，才能进行光源的解码。
[0097]
惯性导航的输入信息是相机在相机坐标系的运动，一般而言会输入六个量：偏航、俯仰、滚转的角速度以及xyz三个方向上的加速度。由于相机连续拍摄两帧之间的时间间隔极短，因此可以认为相机只发生了姿态的变化，且认为是匀速，位置的变化忽略不计。
[0098]
设相机的内参数矩阵为光源在导航坐标系下的三维坐标为其中i代表光源序号；tn为相机在导航坐标系下的位置，rk为相机在拍摄第k帧图像时的绝对姿态，则根据相机的投影成像模型，有：
[0099][0100][0101]
式中也叫深度因子；(u
i,k
,v
i,k
)为第i号光源在第k帧时的二维图像坐标，是其在相机系下的三维坐标。
[0102]
设相机以[ω
x
,ωy,ωz]的角速度运动，该角速度以相机系为坐标基准，拍摄连续两帧之间的时间间隔为δt，则第k帧时的相机到第k+1帧时的姿态转换矩阵为：
[0103][0104]
为了后续表达的整洁，将式(27)写为：
[0105]
[0106]
于是在第k+1帧时有：
[0107][0108][0109]
联立式(26)(29)(30)，得到：
[0110][0111]
再将k的具体形式代入式(31)，得到：
[0112][0113]
由式(26)注意到有代入(32)最终得到：
[0114][0115][0116]
光源二维像素坐标与相机的运动均为已知量，由此得到了第k帧的每个星点到第k+1帧的配准量，从而可以将拍摄得到的每一帧星点均配准至同一时刻的位置，使解码能够在准静态下进行。
[0117]
步骤五：光源识别
[0118]
在准静态情况下，同一星点的二维质心坐标只在某一特定小范围内变化。对这些范围求并集，得到通信域，然后判断配准后的质心坐标是否在选定区域内，得到该星点对应光源的闪烁编码，最后与接收机内部储存的光源闪烁编码进行匹配，可以得到该光源在导航坐标系下的三维坐标。
[0119]
判断二进制序列的相似性可以采用互相关的方法，求取两个离散序列的互相关函数：
[0120]
r(n)＝∑m[x(m)y(n+m)](35)
[0121]
式中：x和y代表离散序列，m表示整个可能的整数求和区域。
[0122]
将闪烁编码与存储的目标星编码进行互相关运算，通过比较互相关函数的最大值可以得到相似性最高的一组序列，该序列所对应的星点就是目标星代号，从而光源被成功识别，得到了光源对应三维坐标。
[0123]
步骤六：位姿解算
[0124]
利用牛顿-拉夫逊公式迭代求解，共有六个待求量，即位移tn＝[x
nynzn
]
t
，偏航角α，俯仰角β，滚转角γ，因此需要至少六个方程。为保证牛顿-拉夫逊迭代公式收敛至正确值，应寻求尽可能多的方程约束。就该算法而言，约束越多，方程对初值越不敏感。由于光源在不断闪烁，因此一帧图像极可能星点数目少，导致约束方程不足。为保证算法鲁棒性，需要利用多帧图像的星点信息以及惯导信息进行位姿求解。
[0125]
在这里使用三帧信息进行求解。假设要求解第k帧时的相机位姿，那么则利用第k-2帧至第k帧的信息进行求解。同样可以认为相机只有姿态上的变化。由式(23)，并为了表述的简洁，设第k帧时的相机姿态为：
[0126][0127]
首先利用第k帧的图像信息列方程。对某一星点，已知其对应光源的三维坐标，将其二维图像坐标归一化，即：
[0128][0129]
上式与(25)(26)联立得：
[0130][0131]
设将rk、tn具体形式代入(38)，并利用式子第三行消去前两行的深度因子，可以得到：
[0132][0133]
或写成：f
k，1
(θ)＝0(39)
[0134]
式中θ＝[α，β，γ，xn，yn，zn]
t
为待求列向量。类似的，对于其他星点可以求出f
k，2
(θ)＝0，f
k，3
(θ)＝0，
…
[0135]
对于第k-1帧、第k-2帧而言，利用惯导信息及式(27)，可以求出姿态转移矩阵于是有：
[0136][0137]rk-1
、r
k-2
矩阵的各项依旧只是α，β，γ的函数，于是类似于式(37)(38)的过程，可以列出：
[0138]fk-1，1
(θ)＝0，f
k-1，2
(θ)＝0，
…
(41)
[0139]fk-2，1
(0)＝0，f
k-2，2
(θ)＝0，
…
(42)
[0140]
将式(39)(41)(42)组合，得到；
[0141][0142]
由此得到了牛顿-拉夫逊迭代的目标函数f(θ)。设f(θ)的雅克比矩阵为j，则迭代公式为：
[0143]
θ
k+1
＝θ
k-(j
t
j)-1jt
f(θk)(44)
[0144]
式中j代表j(θk)。由此得到了用户在导航坐标系下的位姿。
[0145]
综上所述，本发明的实施步骤流程如附图3所示。通过上述流程提出了一种动态场景下基于闪烁光信号的定位定姿算法，通过星点提取获得光源质心位置，再通过光源识别获取光源三维位置，从而进行位姿解算。该算法充分利用惯性导航数据与光源的闪烁特性，能够避免与光源之间需要通信的问题，保证算法的鲁棒性，并在用户姿态变化的条件下实现较高精度的定位定姿。
[0146]
相关文献
[0147]
文献[1]采用光流跟踪特征点的方法实现基于多状态约束卡尔曼滤波器的实时视觉辅助惯性导航算法。
[0148]
文献[2]采用共面四点分布的光源作为特征点，使用lut算法实现了单目视觉位姿估计。
[0149]
参考文献：
[0150]
[1]a.i.mourikisands.i.roumeliotis,"amulti-stateconstraintkalmanfilterforvision-aidedinertialnavigation,"proceedings2007ieeeinternationalconferenceonroboticsandautomation,2007,pp.3565-3572.
[0151]
[2]徐伟高.基于单目视觉的位姿测量关键技术研究[d].中国科学院大学(中国科学院西安光学精密机械研究所),2016。