一种融合机载多传感器的旋翼无人机自主定位方法与流程
本发明涉及无人机的技术领域，更具体地，涉及一种融合机载多传感器的旋翼无人机自主定位方法。
背景技术：
在缺乏外界定位系统的情况下，例如gps或室内定位系统，依靠无人机的机载传感器系统实现其位置与姿态的实时估计是具有挑战性的问题。无人机的自定位解决方案与机载传感器的类型紧密相关，针对不同的无人机机载传感器系统目前存在如下估计无人机自定位信息的机载传感器系统。
1)单/双目视觉系统与惯性测量单元(惯性测量单元)的组合；
2)激光测距仪与惯性测量单元的组合；
3)rgb-d摄像机与惯性测量单元的组合。
考虑传感器的成本与重量，利用单目视觉与惯性测量单元作为机载传感器以估计旋翼无人机的自定位信息是适宜小型无人机平台的方案，尤其是融合利用单目视觉估计相对速度的光流法与利用惯性测量单元进行姿态解算算法的方法。其中光流法无需对图像进行特征提取与特征匹配，而是考虑图像灰度在时间上的变化率与摄像头和图像场景之间相对变化的关系。除了灰度信息，光流法要求提供图像像素对应的深度信息，从而完成对无人机位姿的解算。但是，仅采用光流法存在位置估计的累积误差，在无人机长距离飞行的情况下，其定位误差将逐渐累积，因此，融合光流法与利用惯性测量单元解算无人机姿态的方法，将有效提高无人机依靠轻量机载传感器系统实现较长距离自定位的精度。
技术实现要素：
本发明为克服上述现有技术所述的至少一种缺陷，提供一种融合机载多传感器的旋翼无人机自主定位方法，本方法在无gps或室内定位系统等外界定位系统的情况下，能实现无人机较长时间内的准确自定位。
为解决上述技术问题，本发明采用的技术方案是：一种融合机载多传感器的旋翼无人机自主定位方法，其中，包括以下步骤：
s1.利用机载摄像头，获取地面的实时图像信息；
s2.实时获取地面纹理灰度图像信息，对每帧灰度图像选择固定数量的特征点，通过匹配相邻两帧灰度图像，计算光流向量，并通过差分，获取光流速度；
s3.同步摄像头、惯性测量单元与超声波传感器的时间戳，利用惯性测量单元获取的实时姿态信息，对光流速度进行姿态补偿，得到光流速度在水平方向上的分量；利用超声波传感器获取的高度信息对光流速度在水平方向熵的分量进行尺度还原，得到无人机的水平方向上的速度；
s4.对无人机在水平方向上的速度进行积分得到相对位移信息，累加相对位移信息，则得到水平方向上的位移信息。
进一步的，所述的步骤s1中，
考虑包括加速度计与陀螺仪的惯性测量单元，利用机载惯性测量单元模块中的加速度计和陀螺仪分别测得无人机的实时加速度和角速度信息，通过四元数解算，估计旋翼无人机的实时姿态信息，其中实时无人机的姿态信息包括无人机的偏航角增量、横滚角和俯仰角；
利用机载超声波传感器测量无人机相对于地面的高度信息，并利用惯性测量单元获取的实时姿态信息进行姿态补偿，实时校正垂直高度信息。
所述的步骤s2中，
光流速度的获取方法为：
对每帧灰度图采用shi-tomasi角点检测的方法，选取100个纹理信息最明显的特征点，以特征点为中心选取3*3的像素窗口作为一个像素单元，将前一帧灰度图中的像素窗口位置作为后一帧灰度图的像素窗口的初始位置，建立一个搜索区域，利用lucas-kanade反向相乘算法，采用五层光流金字塔模型，利用最小二乘法，通过令前一帧的像素窗口在后一阵的灰度图的搜索区域内搜索灰度差和最小，求得后一帧的像素窗口位置，两帧像素窗口的距离差，即为光流向量，通过差分，获取光流速度。
所述的步骤s3中，姿态补偿的方法为：包括数据表示和变换表示。
所述的步骤s3中，光流速度的获取方法为：
通过摄像机投影矩阵将图像坐标系下的二位点与摄像机坐标系下的三维点进行匹配；
通过变换矩阵将摄像机坐标系下的空间点转换到机体坐标系下；在坐标变换的过程中消除摄像头不在机体中心而造成的偏差；
通过变换矩阵将机体坐标系下的空间点转换到世界坐标系下，在缺乏磁力计，无法获取当前时刻方位角的情况下，将世界坐标系的x轴正方向设为初始飞行机头的朝向方向。
所述的步骤s4中，对无人机在水平方向上的速度进行积分得到相对位移信息，累加相对位移信息，即得水平方向上的相对位置信息。
与现有技术相比，有益效果是：本发明使用lk光流反向相乘算法结合金字塔算法估计光流，避免了传统光流算法迭代中反复计算海森矩阵的步骤，简化了的迭代过程，降低了算法的复杂度。同时金字塔算法为由粗到精的估计策略，很大程度上克服了较大、较快的运动所造成的光流估计错误问题。从而使算法能应用到飞行速度较快的旋翼无人机平台上。
本发明采用对极几何模型，融合光流估计、无人机的姿态角以及飞行高度信息估计无人机在水平方向上的飞行速度。通过对对极几何模型进一步地推导，在没有偏航角信息的情况下，本发明利用惯性测量单元模块解算出两帧图像之间的偏航角增量，并将之用于对光流速度进行姿态补偿，估计旋翼无人机在世界坐标系内的水平飞行速度，减少平台所搭载的传感器数量。
附图说明
图1为本发明无人机机体坐标系与摄像机坐标系示意图；
图2为本发明无人机水平方向速度的估计流程；
图3为本发明基于多传感器的旋翼无人机实时自主定位方法流程图；
图4为本发明实现无人机定位的场景图。
具体实施方式
附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。
一种融合机载多传感器的旋翼无人机自主定位方法，包括：
1)利用机载摄像头，获取地面的实时图像信息；
考虑包括加速度计与陀螺仪的惯性测量单元，利用机载惯性测量单元模块中的加速度计和陀螺仪分别测得无人机的实时加速度和角速度信息，通过四元数解算，估计旋翼无人机的实时姿态信息，其中实时无人机的姿态信息包括无人机的偏航角增量、横滚角和俯仰角；
利用机载超声波传感器测量无人机相对于地面的高度信息，并利用惯性测量单元获取的实时姿态信息进行姿态补偿，实时校正垂直高度信息；
2)实时获取地面纹理灰度图像信息，对每帧灰度图像选择固定数量的特征点，通过匹配相邻两帧灰度图像，计算光流向量，并通过差分，获取光流速度；
3)同步摄像头、惯性测量单元与超声波传感器的时间戳，利用惯性测量单元获取的实时姿态信息，对光流速度进行姿态补偿，得到光流速度在水平方向上的分量；利用超声波传感器获取的高度信息对光流速度在水平方向熵的分量进行尺度还原，得到无人机的水平方向上的速度；
4)对无人机在水平方向上的速度进行积分得到相对位移信息，累加相对位移信息，则得到水平方向上的位移信息，
所述的步骤2)中光流速度的获取方法为：
对每帧灰度图采用shi-tomasi角点检测的方法，选取100个纹理信息最明显的特征点，以特征点为中心选取3*3的像素窗口作为一个像素单元，将前一帧灰度图中的像素窗口位置作为后一帧灰度图的像素窗口的初始位置，建立一个搜索区域，利用lucas-kanade反向相乘算法，采用五层光流金字塔模型，利用最小二乘法，通过令前一帧的像素窗口在后一阵的灰度图的搜索区域内搜索灰度差和最小，求得后一帧的像素窗口位置，两帧像素窗口的距离差，即为光流向量，通过差分，获取光流速度。
lk反向相乘算法如下：
其中，w(x；p)为扭曲因子，即w(x,p)＝(x+p1,y+p2)t，其中p＝(p1,p2)t为前后两帧灰度图像上像素窗口在图像x和y方向的光流偏移t(x)与i(x)分别代表当前时刻与上一时刻的两帧灰度图。
利用迭代
更新扭曲因子矩阵w(x；p)。
将式(2.1.1)关于p＝0处进行一阶泰勒展开得：
对上式对δp求导，并令导数为0，则增量δp可表示为：
其中，
由于雅各比行列式
进一步，步骤3)中所述姿态补偿的方法为：
1)对于数据表示。如图1所示，记前后两个时刻为t1和t2，对应的超声波测量信息为l1和l2。
对于惯性测量单元姿态角，建立机体坐标系：
x轴正方向对应机体右侧，该轴对应的欧拉角为pitch角(俯仰角)；
y轴正方向对应机头方向，该轴对应的欧拉角为roll角(滚转角)；
z轴正方向对应竖直向上，该轴对应的欧拉角为yaw角(偏航角)。
无人机欧拉角的合成顺序(即通过欧拉角得到旋转矩阵的顺序)为一般意义下的偏航角-俯仰角-滚转角(yaw-pitch-roll)。记无人机在某个时刻ti在世界坐标系中测得的偏航角(yaw)、俯仰角(pitch)和滚转角(roll)为αi、βi和γi，那么无人机机体坐标系在该时刻相对于世界坐标系的旋转变换关系为：
其中
(2)对于变换表示。
这里一个空间三维点的齐次坐标表示记为x＝[x,y,z,1]t，在对于数据的符号表示中，我们用oi表示ti时刻的机体坐标系，所以在oi坐标系下的三维点齐次坐标记为xoi，不同的坐标系下标表示这个三维点坐标的值是相对于不同空间坐标系的，下文均按照这个记法进行类似的表示。
这里记在t1和t2时刻的图像平面对应点齐次坐标表示分别为：x1＝[x1,y1,1]t和x2＝[x2,y2,1]t(这两个投影结果可以通过上述光流算法得到)，并设该平面对应点在t1时刻的机体坐标系o1下的三维空间点齐次坐标为xo1，则存在摄像机投影矩阵p1和p2使得：
x1＝p1xo1(7)
x2＝p2xo1(8)
其中：
其中，k为摄像机内参矩阵，
其中，tc1(o1)、tc2(o2)分别表示机体坐标系o1、o2在摄像机坐标系c1中的三维坐标。矩阵
我们能通过图像和解算的姿态估计相机坐标系和摄像机坐标系的旋转关系；
在图1所示的摄像机坐标系中，以相机为原点，测量惯性测量单元模块到摄像头中心的偏移可得到tc1(o1)。
另外，等式(10)中的
其中
to2(o1)为待求位移向量，表示机体坐标系o1的原点在机体坐标系o2中三维坐标。
所述的步骤3)中光流速度的获取方法为：
如图2所示，在本发明无人机水平速度的估计流程图中：
①通过摄像机投影矩阵k[i|0]将图像坐标系下的二位点与摄像机坐标系下的三维点进行匹配；
②通过变换矩阵
③通过变换矩阵
为了获得水平速度估计，需要计算t1和t2时刻之间水平姿态下的机体位移(这里均以机载关心测量单元作为机体运动的度量)，引入新的相机坐标系o1-，o1-表示t1时刻的相机坐标系o1校正滚转角γ1和俯仰角β1后的机体坐标系，即有：
t1和t2时刻图像上的每组平面对应点都有一个三维空间点与之对应，将三维空间点在相机坐标系o1-下的三维点齐次坐标记为xo1-，由于已经校正了滚转角和俯仰角，所以xo1-中的“z”坐标分量均为t1时刻的高度(这是由最开始的基本假设得到的：速度估计使用的图像纹理信息均近似在同一水平面)，即：
xo1-＝[x,y,-h1,1]t
其中h1＝l1cos(β1)cos(γ1)，l1为t1时刻的超声波测量信息。
由此，t1和t2时刻的图像对应点成像映射表示为：
对等式(14)和(15)进行矩阵运算化简得到：
由上可知，x1、x2、k、
简记为：
其中
其中to1(c1)＝to2(c2)，可以根据机体坐标系，以惯性测量单元模块的中心点为原点，通过测量摄像头到惯性测量单元模块中心点的偏移得到。
上式(20)和(21)中，只有to1-(o2)为待求量，这个待求量的含义就是：在t1时刻机体惯性测量单元的水平姿态坐标系o1-下(含偏航角姿态，但不含俯仰角和滚转角姿态)，t2时刻的机体惯性测量单元三维偏移量。只要求得了求得这个三维偏移量，除以t1和t2时刻的时间差就可以得到水平方向上的速度。
由前面式子可得，to1-(c1)和
其中，dh也是可以求得的：dh＝h2-h1＝l2cos(β2)cos(γ2)-l1cos(β1)cos(γ1)
由于
所以利用已知的深度信息：-h1-zt1和-h1-zt2-dh，有：
由于上式中xt1、yt1、xt2和yt2均为已知量，可易得水平姿态下的偏移量[dx,dy]，最后所求的水平速度为：
求得所有特征点像素串口对应的飞行器速度估值[vxi,vyi]后，通过中值滤波取得速度估值的中位数[vmid,vmid]为最终的速度估计。
通过设置具有普适性的阈值，计算所有速度估值点[vxi,vyi]到中位数点[vmid,vmid]的距离小于该阈值的数量，以此数量作为评估速度估计结果的依据，当点的数量占比达到70％时，则判定速度估计结果正确，否则判定估计结果失败，舍弃此次结果，进入下一次估计。
所述步骤4)对无人机在水平方向上的速度进行积分得到相对位移信息，累加相对位移信息，即得水平方向上的相对位置信息。
本发明适用环境如图4所示，旋翼无人机在水泥地面、木制地板等有规则和纹理地面条件下的飞行示意图。下标w的坐标系意为世界坐标系，即在整个场景下建立的全局坐标系。无人机上下标为c的坐标系意为机体坐标系，由于设计了多传感器固连装置，惯性测量单元、摄像头、超声波传感器、机体均为刚性连接，因此各自坐标系只存在一个固定的坐标转换关系，该关系在设计固连装置时已给定，如此解决了各传感器之间的坐标转换问题。坐标轴x，y为图像坐标系，任意地面一点投影到图像平面将在该平面建立一个像素单位的坐标。相机为下视视野，能够向下观测到部分地面的纹理信息。
本发明涉及融合机载多传感器数据的旋翼无人机实时自主定位技术，利用单目摄像头获取图像，并结合多个机载传感器，估计无人机在水平方向上的飞行速度，通过对速度进行积分得到无人机相对起始点的水平位移，从而实现无人机自主实时定位。本发明采用实时性能较高的lk光流反向迭代算法计算光流，利用对极几何模型，并在偏航角未知的情况下，利用偏航角增量估计无人机的实时姿态信息，从而对光流速度进行姿态补偿，提出了一种实时、准确的旋翼无人机自主定位解决方案。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！