基于概率膜计算的煤矿井下无人直升机SLAM

基于概率膜计算的煤矿井下无人直升机slam
技术领域
1.本发明涉及无人机监测技术领域，具体涉及基于概率膜计算的煤矿井下无人直升机slam。


背景技术：

2.在未知环境下，无人机可以对自身位置和姿态进行估计，是能够有效完成任务的关键技术。当无人机在低空，低速行驶时，使用全球定位系统(global positioningsystem，gps)和惯性导航系统(inertial navigation system，ins)等方法可以获得飞行所需的位置、速度等信息，满足在城市中的飞行需求。但是gps和ins存在使用的局限性，gps的误差范围为3-10米，而且在室内环境和高山、峡谷等恶劣环境下由于信号弱导致无法正常工作，尤其是近年来无人机发展迅速在开采领域也取得了一定成就，尤其是露天矿场和地下掘进巷道，无人机具有速度快、精度高等优点，可以在复杂环境下快速完成数据采集，地形勘探，但是由于矿场环境复杂，传统的定位装置精度难以保证，因此设计一种多数据融合，可利用无人机自带的传感器进行定位和飞行姿态调整的方法就显得尤为重要。


技术实现要素：

3.针对上述技术背景提到的不足，本发明的目的在于提供基于概率膜计算的煤矿井下无人直升机slam。
4.本发明的目的可以通过以下技术方案实现：
5.基于概率膜计算的煤矿井下无人直升机slam，其主要包括传感器模型、基于概率稀疏扩展slam、概率膜计算slam，所述传感器模型是在分析各传感器工作原理的基础上，给出相应传感器数学模型；其次，设计膜内概率膜算法，构建概率膜系统，基于稀疏滤波实现概率slam地图构建。
6.进一步的，所述传感器模型包括惯性测量单元、激光雷达、深度相机，惯性测量单元由三个线性、正交的加速度计和陀螺仪构成，可测量正交坐标轴下的角速率和加速度；激光雷达作为利用激光脉冲实现位置测量的传感器，通过控制激光束反射角度来描述方位角及俯仰角，深度相机可测量像素的深度。
7.进一步的，所述基于概率稀疏扩展slam，具体扩展方法如下：
8.当无人直升机不清楚自身位置，无法获取环境地图，所有的数据集中于测量数据和控制数据，从概率学角度，slam问题被划分为全slam和在线slam，在全slam中，除了直升机位姿，还需要计算路径和后验地图，时间t时刻的概率可表达为：
9.p(x
1～t
,m|y
1～t
,u
1～t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)在线slam，考虑即时位姿和地图后验，时间上的概率表达为：
10.p(x
t
,m|y
1～t
,u
1～t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
11.式(13)和式(14)中，x
t
表示t时刻直升机位姿，m表示地图，y
1～t
,u
1～t
为测量和控制数据，其分别表示的模型如图4所示，
12.在实际应用过程中，对全slam的过去状态积分可实现在线slam，积分呈现连续状态，依次进行，如式(15)所示，
13.p(x
t
,m|y
1～t
,u
1～t
)＝∫∫
···
∫p(x
1～t
,m|y
1～t
,u
1～t
)dx1dx2···
dx
t-1
ꢀꢀꢀꢀꢀꢀ
(15)
14.基于连续和离散的问题，连续问题包含地图中无人机位姿和定位，在特征表示过程中物体以信标等表示，而离散特性与一致性关联，检测到物体时，算法会计算前后检测到物体之间的关联，表现出离散特征，即”0”或”1”状态，因此，一致性的明确很必要，结合式(13)和式(14)，加入一致性变量的在线和全slam如下：
[0015][0016]
式(16)中，f
t
为一致性变量下对应的矢量，
[0017]
稀疏扩展滤波：与扩展卡尔曼滤波相比，出于在线运行和计算高效率目的，稀疏扩展滤波表征了信息的高效性，继承无人直升机位姿和地图后验证，即通过非零元素相连接的维持稀疏矩阵，稀疏扩展滤波的计算过程包含测量更新、运动更新、稀疏化和估计四部分，
[0018]
运动更新使用矩阵的稀疏性，在时间上不依赖地图的规模，把信息矩阵ω和矢量ζ通过更新来完成对控制的处理，根据卡尔曼滤波给出：
[0019][0020][0021]
式(17)和式(18)中，σ表示协方差矩阵，f
x
表示直升机状态矢量矩阵，g
t
表示雅可比矩阵表示对时间t的导数，表示时间t时刻的估计均值，其中g
t
，f
x
，δ的表示如下：
[0022][0023][0024][0025]
根据式(18)和式(19)可推导出：
[0026][0027]
式(22)中信息矩阵ω维数随机且在有限时间实现，假设信息矩阵ω稀疏，则更新效率增强，定义：
[0028][0029]
将式(23)代入式(22)可得：
[0030][0031]
由矩阵逆引理可进一步得到：
[0032][0033]
假设有限时间内根据ω计算得到φ
t
，则存在时间有限条件下的计算可行性，利用直升机位姿和地图特征的矩阵元素(非零)，稀疏条件下不依赖ω的大小，考虑g
t
的逆可以由以下计算得出：
[0034][0035]
式(26)中，对应地图特征元素非零，
[0036]
测量更新考虑直升机飞行过程中的滤波更新，通过扩展卡尔曼滤波实现：
[0037][0038]
其中，q
t
为噪声协方差矩阵。
[0039]
进一步的，所述稀疏扩展滤波对信息矩阵ω稀疏化是必要的，通过稀疏化的表征，保障其后验分布处于稀疏状态，基于此，剔除直升机位姿和地图特征之间的关联，进一步限制了特征间的数量，为实现上述思想，引入两种新的连接，首先，依靠非活动连接激活此特征，在无人直升机位姿和特征之间引入新连接；其次，直升机运动引入活动特征的两个新连接，限制活动特征的数量，避免出现两个非稀疏边界，此时的稀疏通过少的活动特征获取，
[0040]
稀疏化定义过程中将特征集合分为3个子集(不相交)：
[0041]
m＝m0 m1 m2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(28)
[0042]
式(28)，m1为活动继续的特征集合，m0为即将激励的活动特征，m2为非活动特征，在稀疏化步骤中继续非活动状态，同时删除直升机位姿和m0之间的连接，将稀疏化引入后验，因m1和m0包含当前所有特征，后验p(y
t
|z
1～t
,u
1～t
,f
1～t
)可表征如下：
[0043][0044]
式(29)中，x
t
不依赖非活动特征m2的前提是m0和m1已知，故m2可取随机值，利用通用项稀疏化规约，降低对m0依赖，取m2＝0，则：
[0045][0046]
式(30)中，z
1～t
表示直到t时刻的测量值，u
1～t
在线slam控制量。
[0047]
进一步的，所述概率膜计算slam具体步骤如下：
[0048]
确定无人直升机的实时位置，膜控制器开始执行的每个周期都接收机载传感器数据(x，y，θ)
t
,位置更新输出数据(x
′
，y
′
，θ
′
)
t
，根据膜计算的分布并行特征，建立度为4的概率膜系统，
[0049]
π＝(m，μ，w1，w2，w3，r，{cr}
r∈r
)
ꢀꢀꢀꢀꢀꢀꢀ
(31)
[0050]
1)m＝{x
ij
，y
ij
，θ
ij
，error：i，j∈[1，2]}；m表示以x
ij
,y
ij
,θ
ij
,error为对象构成的集合，其中：x
ij
表示无人直升机不同位置的横坐标，y
ij
表示无人直升机不同位置的纵坐标，θ
ij
表示无人直升机对应x
ij
,y
ij
坐标下的方位角，error为无人直升机运动产生的误差；
[0051]
2)μ＝[[[]2[]3]4]1，μ表示无人直升机组织膜系统膜结构，其中：[]表示单个膜，1，2，3，4分别表示膜的标记，即第i个膜(i＝1,2,3,4)；
[0052]
3)w1＝p(x
t
|u
t
,x
t-1
),w1表示解算密度函数，其中：x
t
为无人直升机t时刻姿态，u
t
为无人直升机控制输入，x
t 1
为t 1时刻无人直升机位姿；
[0053]
4)w2＝p(x
t
′
|u
t
′
,x
t-1
′
),w2表示噪声干扰后密度计算，其中：x
t
′
为无人直升机在噪声干扰后t时刻姿态，u
t
′
为噪声干扰控制输入，x
t 1
′
为无人直升机在噪声干扰后t 1时刻姿态；
[0054]
5)w3表示理想密度评价，其中：为无人直升机t时刻理想姿态，为无人直升机理想位姿对应的控制输入，为t 1时刻无人直升机理想位姿；
[0055]
6)r为规则集，转移过程中保持能量守恒；
[0056]
7)cr表示基于r规则集下的对象产生进化的概率，
[0057]
概率膜计算模型：结合概率膜计算方法，对排除m2的所有变量服从p(x
t
,m0,m1|m2＝0)分布计算矩阵提取状态变量子矩阵：
[0058][0059]
式(32)中，f
x
表示直升机状态矢量矩阵，m0为活动继续的特征集合，m1为即将激励的活动特征，根据矩阵引理，对p(m1|m2,z
1～t
,u
1～t
,f
1～t
)和p(x
t
,m0,m1,m2|z
1～t
,u
1～t
,f
1～t
)矩阵信息进行如下定义：
[0060][0061]
式(33)中的f为把全部状态投影成包含变量子集状态的投影阵，同理，p(m0,m1,m2|z
1～t
,u
1～t
,f
1～t
)可转换成矩阵：
[0062][0063]
将式(32)和式(34)合并可得到信息矩阵和信息矢量分别为：
[0064][0065][0066]
进一步的，所述膜计算模型中个算法执行过程如下：
[0067]
传感数据更新算法：
[0068][0069]
直升机运动更新算法：
[0070]
[0071][0072]
直升机状态估计更新算法：
[0073][0074]
根据无人直升机后验输入ω
t
和ξ
t
，位移d和角向量α确定坐标系下的位置，同时将位移和向量映射到机体坐标系，基于旋转和平移，直升机位姿和地图特征分别表示为：
[0075][0076][0077]
式(39)和式(38)包含矩阵和向量的旋转与平移，下一步给出两式的证明，定义δ：
[0078][0079]
δ＝[d
x d
y α]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(40)
[0080]
将式(39)和式(38)进行状态向量推导得：
[0081][0082]
根据空间坐标变换相似性原理，进一步通过平移和旋转，信息矩阵和向量定义直升机随时间的后验：
[0083][0084]
由于对有：
[0085][0086]
融合过程中存在数据等价问题，通过增加约束进一步控制，增加地图特征惩罚矩阵c，其值愈大约束愈强，通过联合后验概率实现地图融合，融合算法如下：
[0087][0088]
本发明的有益效果：
[0089]
本发明在分析得出lidar、mu和深度相机传感器数学模型的基础上，设计了概率膜系统模型和膜算法，并对建立的模型和算法进行了验证，通过理论分析并与实验相结合，将本发明提出的方法与单一、两种和三种传感器融合的建图方法进行比较，验证了概率膜计算在建图效果上具备很好的性能。
附图说明
[0090]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图；
[0091]
图1是概率膜系统slam流程图；
[0092]
图2是激光雷达示意图；
[0093]
图3是深度相机模型；
[0094]
图4是加固连接装置的结构示意图；
[0095]
图5是膜计算模型p-lingua文件框架；
[0096]
图6是不同slam算法的角度误差比较图；
[0097]
图7是不同slam算法绝对误差比较图。
具体实施方式
[0098]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
[0099]
基于概率膜计算的煤矿井下无人直升机slam(probability membrane computing slam,pmc-slam)，其主要包括传感器模型、基于概率稀疏扩展slam、概率膜计算slam。
[0100]
其中传感器模型包括惯性测量单元(imu)、激光雷达(lidar)、深度相机(camera)。
[0101]
惯性测量单元由三个线性、正交的加速度计和陀螺仪构成，可测量正交坐标轴下的角速率和加速度。对惯性测量下的测量值基于传感器自身坐标系，非无人机机体坐标系。假设机体状态向量位姿：{c
vi
,r
vi
}，角速度：w
vi
，加速度：进一步假设传感器和机体坐标系位置关系确定，r
sv
表示旋转，c
sv
表示平移。角速度w与在传感器坐标系下机体的速率关系表示如下：
[0102]
w＝w
vicsv
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0103]
根据加速度测量原理，加速度测量值表示为：
[0104][0105]
表示传感器坐标原点s的加速度，g为重力加速度。由于公式2表示的值并不是机体坐标系下的值，需要将传感器和机体坐标转换，因为：
[0106][0107]
利用泊松公式对式(3)两次求导可得：
[0108][0109]
式(4)右侧由初始参数构成，为传感器原点的关系加速度。将式(4)代入式(2)得：
[0110][0111]
式(5)中，a为加速度计模型，将式(1)和式(5)模型进一步表示成矩阵形式下的imu传感器模型为：
[0112][0113]
式(6)中，g表示惯性坐标系下重力加速度。
[0114]
激光雷达作为利用激光脉冲实现位置测量的传感器，通过控制激光束反射角度来
描述方位角及俯仰角，距离通过飞行时间测得，其表示如图2所示。图2中，p点坐标表示为：
[0115]
p＝r
ps
＝[x,y,z]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0116]
以矩阵形式表示为：
[0117]
p＝c
t
(α)c
t
(ε)[r,0,0]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0118]
式(8)中，r表示距离，ε表示俯仰角，α表示方位角，c为沿坐标轴的旋转矩阵。为进一步表示式(7)和式(8)之间的关系，根据旋转求逆可得：
[0119][0120]
式(9)表明了激光雷达观测点位置(x,y,z)与距离r，俯仰角ε和方位α之间的映射关系。
[0121]
深度相机可测量像素的深度，测量方法如下：p点坐标(x,y,z),左右相机的模型分别定义为：
[0122][0123]
式(10)中，k为相机参数，fu为水平像素焦距，fv为垂直焦距。由于两相机参数矩阵相同，当左相机获取观测值时，右侧相机沿图3中的实直线便可获取右侧相机观测值(垂直)，因此，式(10)进一步组合可得到整体相机的模型为：
[0124][0125]
据式(11)，由于参数矩阵n存在相同的两行，所以n不可逆，约束条件下左右两相机存在c
l.r
＝1的姿态关系，可得到v
l
＝vr，其中v
l
和vr为坐标点x到像素坐标下的映射。
[0126]
信息融合方法，本发明以三种不同的传感器信息为基础分别构建三张地图，再对构建的地图进行融合，由于多传感器之间的数据不相互依赖，假设3种传感器生成的地图表示为map3，利用德摩根定律分解可得：
[0127]
p(map)＝1-π(1-p(map3))
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0128]
如果式(12)中的激光雷达、imu或深度相机传感器构建的地图被划分单元占用，则融合地图呈现相同的占用状态。
[0129]
基于概率稀疏扩展slam：
[0130]
当无人直升机不清楚自身位置，无法获取环境地图，所有的数据集中于测量数据和控制数据。从概率学角度，slam问题被划分为全slam和在线slam。在全slam中，除了直升
机位姿，还需要计算路径和后验地图，时间t时刻的概率可表达为：
[0131]
p(x
1～t
,m|y
1～t
,u
1～t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)在线slam，考虑即时位姿和地图后验，时间上的概率表达为：
[0132]
p(x
t
,m|y
1～t
,u
1～t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0133]
式(13)和式(14)中，x
t
表示t时刻直升机位姿，m表示地图，y
1～t
,u
1～t
为测量和控制数据，其分别表示的模型如图4所示。
[0134]
在实际应用过程中，对全slam的过去状态积分可实现在线slam，积分呈现连续状态，依次进行，如式(15)所示。
[0135]
p(x
t
,m|y
1～t
,u
1～t
)＝∫∫
···
∫p(x
1～t
,m|y
1～t
,u
1～t
)dx1dx2···
dx
t-1
ꢀꢀꢀꢀꢀꢀ
(15)
[0136]
基于连续和离散的问题，连续问题包含地图中无人机位姿和定位，在特征表示过程中物体以信标等表示。而离散特性与一致性关联，检测到物体时，算法会计算前后检测到物体之间的关联，表现出离散特征，即”0”或”1”状态。因此，一致性的明确很必要，结合式(13)和式(14)，加入一致性变量的在线和全slam如下：
[0137][0138]
式(16)中，f
t
为一致性变量下对应的矢量。
[0139]
稀疏扩展滤波：与扩展卡尔曼滤波相比，出于在线运行和计算高效率目的，稀疏扩展滤波表征了信息的高效性，继承无人直升机位姿和地图后验证，即通过非零元素相连接的维持稀疏矩阵。稀疏扩展滤波的计算过程包含测量更新、运动更新、稀疏化和估计四部分。
[0140]
运动更新使用矩阵的稀疏性，在时间上不依赖地图的规模，把信息矩阵ω和矢量ζ通过更新来完成对控制的处理，根据卡尔曼滤波给出：
[0141][0142][0143]
式(17)和式(18)中，σ表示协方差矩阵，f
x
表示直升机状态矢量矩阵，g
t
表示雅可比矩阵表示对时间t的导数，表示时间t时刻的估计均值。其中g
t
，f
x
，δ的表示如下：
[0144][0145][0146]
[0147]
根据式(18)和式(19)可推导出：
[0148][0149]
式(22)中信息矩阵ω维数随机且在有限时间实现，假设信息矩阵ω稀疏，则更新效率增强。定义：
[0150][0151]
将式(23)代入式(22)可得：
[0152][0153]
由矩阵逆引理可进一步得到：
[0154][0155]
假设有限时间内根据ω计算得到φ
t
，则存在时间有限条件下的计算可行性。利用直升机位姿和地图特征的矩阵元素(非零)，稀疏条件下不依赖ω的大小，考虑g
t
的逆，可以由以下计算得出：
[0156][0157]
式(26)中，对应地图特征元素非零。
[0158]
测量更新考虑直升机飞行过程中的滤波更新，通过扩展卡尔曼滤波实现：
[0159][0160]
其中，q
t
为噪声协方差矩阵。
[0161]
稀疏扩展滤波对信息矩阵ω稀疏化是必要的，通过稀疏化的表征，保障其后验分布处于稀疏状态，基于此，剔除直升机位姿和地图特征之间的关联，进一步限制了特征间的数量。为实现上述思想，引入两种新的连接。首先，依靠非活动连接激活此特征，在无人直升机位姿和特征之间引入新连接；其次，直升机运动引入活动特征的两个新连接，限制活动特征的数量，避免出现两个非稀疏边界，此时的稀疏通过少的活动特征获取。
[0162]
稀疏化定义过程中将特征集合分为3个子集(不相交)：
[0163]
m＝m0 m1 m2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(28)
[0164]
式(28)，m1为活动继续的特征集合，m0为即将激励的活动特征，m2为非活动特征，在稀疏化步骤中继续非活动状态，同时删除直升机位姿和m0之间的连接。将稀疏化引入后验，因m1和m0包含当前所有特征，后验p(y
t
|z
1～t
,u
1～t
,f
1～t
)可表征如下：
[0165][0166]
式(29)中，x
t
不依赖非活动特征m2的前提是m0和m1已知，故m2可取随机值。利用通用
项稀疏化规约，降低对m0依赖，取m2＝0，则：
[0167][0168]
式(30)中，z
1～t
表示直到t时刻的测量值，u
1～t
在线slam控制量。
[0169]
概率膜计算slam：在概率膜计算中，规则以概率方式执行，可以快速处理数据，结合膜计算的并行、分布式特征，构成一个独立、协作的数据处理膜计算模型。采用概率膜系统对煤矿井下无人直升机环境感知地图构建流程如图1所示。
[0170]
确定无人直升机的实时位置，膜控制器开始执行的每个周期都接收机载传感器数据(x，y，θ)
t
,位置更新输出数据(x
′
，y
′
，θ
′
)
t
。根据膜计算的分布并行特征，建立度为4的概率膜系统。
[0171]
π＝(m，μ，w1，w2，w3，r，{cr}
r∈r
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(31)
[0172]
式(31)中：
[0173]
1)m＝{x
ij
，y
ij
，θ
ij
，error：i，j∈[1，2]}；m表示以x
ij
,y
ij
,θ
ij
,error为对象构成的集合。其中：x
ij
表示无人直升机不同位置的横坐标，y
ij
表示无人直升机不同位置的纵坐标，θ
ij
表示无人直升机对应x
ij
,y
ij
坐标下的方位角，error为无人直升机运动产生的误差；
[0174]
2)μ＝[[[]2[]3]4]1，μ表示无人直升机组织膜系统膜结构，其中：[]表示单个膜，1，2，3，4分别表示膜的标记，即第i个膜(i＝1,2,3,4)；
[0175]
3)w1＝p(x
t
|u
t
,x
t-1
),w1表示解算密度函数。其中：x
t
为无人直升机t时刻姿态，u
t
为无人直升机控制输入，x
t 1
为t 1时刻无人直升机位姿；
[0176]
4)w2＝p(x
t
′
|u
t
′
,x
t-1
′
),w2表示噪声干扰后密度计算。其中：x
t
′
为无人直升机在噪声干扰后t时刻姿态，u
t
′
为噪声干扰控制输入，x
t 1
′
为无人直升机在噪声干扰后t 1时刻姿态；
[0177]
5)w3表示理想密度评价。其中：为无人直升机t时刻理想姿态，为无人直升机理想位姿对应的控制输入，为t 1时刻无人直升机理想位姿；
[0178]
6)r为规则集，转移过程中保持能量守恒；
[0179]
7)cr表示基于r规则集下的对象产生进化的概率。
[0180]
概率膜计算模型：结合概率膜计算方法，对排除m2的所有变量服从p(x
t
,m0,m1|m2＝0)分布计算矩阵提取状态变量子矩阵：
[0181][0182]
式(32)中，f
x
表示直升机状态矢量矩阵，m0为活动继续的特征集合，m1为即将激励的活动特征，根据矩阵引理，对p(m1|m2,z
1～t
,u
1～t
,f
1～t
)和p(x
t
,m0,m1,m2|z
1～t
,u
1～t
,f
1～t
)矩阵信息进行如下定义：
[0183]
[0184]
式(33)中的f为把全部状态投影成包含变量子集状态的投影阵。同理，p(m0,m1,m2|z
1～t
,u
1～t
,f
1～t
)可转换成矩阵：
[0185][0186]
将式(32)和式(34)合并可得到信息矩阵和信息矢量分别为：
[0187][0188][0189]
由于概率膜算法循环时间上的有限性，不依赖地图本身规模，同时把控制加入计算估计，通过高效率的计算特征，在建图过程中，矩阵(矢量)中的元素更新过程中表征直升机位置和特征向量，概率膜计算模型p-lingua文件框架如图5所示：
[0190]
图5膜计算模型中各算法执行过程如下：
[0191]
传感数据更新算法：
[0192]
[0193][0194]
直升机运动更新算法：
[0195][0196]
直升机状态估计更新算法：
[0197]
[0198][0199]
根据无人直升机后验输入ω
t
和ξ
t
，位移d和角向量α确定坐标系下的位置，同时将位移和向量映射到机体坐标系，基于旋转和平移，直升机位姿和地图特征分别表示为：
[0200][0201][0202]
式(39)和式(38)包含矩阵和向量的旋转与平移，下一步给出两式的证明。定义δ：
[0203][0204]
δ＝[d
x d
y α]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(40)
[0205]
将式(39)和式(38)进行状态向量推导得：
[0206][0207]
根据空间坐标变换相似性原理，进一步通过平移和旋转，信息矩阵和向量定义直升机随时间的后验：
[0208][0209]
由于对有：
[0210][0211]
融合过程中存在数据等价问题，通过增加约束进一步控制，增加地图特征惩罚矩阵c，其值愈大约束愈强。通过联合后验概率实现地图融合，融合算法如下：
[0212][0213]
算法4的执行过程：无人机坐标系间的相对位姿由[d
x d
y α]
t
确定，维持算法稀疏的同时，包含信息矩阵和向量的局部旋转和平移；通过构建联合后验地图实现地图融合，该步骤包含不同地图中对应的特征；执行等价约束，对于相同的两个特征，添加信息矩阵中对这两个特征的连接。
[0214]
利用无人机搭载三种传感器进行信息采集，并将获得的数据进行建图分析。如图6、7所示，其中图6、图7中最下方的线为基于膜概率计算的三数据融合建图后无人机飞行的误差，可以发现利用惯性测量单元(imu)、激光雷达(lidar)和深度相机(camera)进行膜概率计算的三数据融合建图效果明显优于单一传感器或两两组合的传感器数据建图，膜概率计算的三数据融合建图得到的角度误差和绝对误差随着均明显减小，且随着无人机飞行时间和飞行里程数的增加，角度误差和绝对误差均是最小。
[0215]
在本说明书的描述中，参考术语“一个实施例”、“示例”、“具体示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0216]
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。