一种智能船舶增强现实助航信息显示方法与流程

1.本发明涉及智能船舶航行与导航领域，尤其涉及一种智能船舶增强现实助航信息显示方法。


背景技术：

2.随着航运业规模不断扩大，海上和内河航行的船只数量增多，船舶航行安全问题日益凸显，且一系列安全事故分析结果说明了船舶碰撞事故的起因虽然与航行环境和机器设备存在必然关系，但瞭望疏忽或操纵不当仍然是主要因素，保持视觉上的稳定感知对航行安全至关重要。
3.增强现实技术(augmented reality，ar)是一种将真实世界和虚拟世界的信息“无缝”集成的新技术，是把原本在现实世界的一定时间空间范围内很难体验到的实体信息(视觉信息、声音、味道、触觉等)通过计算机技术，模拟仿真后再叠加，将虚拟的信息应用到真实世界，被人类感官所感知，从而达到超越现实的感官体验。2018年是ar技术开启应用的元年，在工业、医疗、文化、娱乐等行业实现专业化和大众化的示范应用。船舶在夜间或能见度不良的情况下航行时，依靠驾驶员目视和相机传感器无法保障稳定的态势感知。驾驶员和岸基安全员一般通过雷达、海图、ais等二维信息进行局面的判断，而ar技术能够以一种直观的视觉显示来辅助安全航行，在保障船舶安全航行方面具有重要意义。然而，现有技术中相机外参估计方法较单一、精度较低，多传感器之间数据通信繁杂，且存在助航信息显示中渲染数据与真实相机参数不对应的问题。


技术实现要素：

4.本发明提供一种智能船舶增强现实助航信息显示方法，以克服现有技术中相机外参估计方法较单一、精度较低，多传感器之间数据通信繁杂，以及助航信息显示中渲染数据与真实相机参数不对应的问题。
5.为了实现上述目的，本发明的技术方案是：
6.一种智能船舶增强现实助航信息显示方法，包括基于ros计算图的多传感器数据通信模块、相机外参估计模块以及ar助航信息显示模块；所述多传感器数据通信模块包括lidar数据通信接点、imu/rtk数据通信节点、三维助航信息通信接点以及相机数据通信节点，包括以下步骤：
7.步骤s1：所述lidar数据通信接点读取船载lidar测量的三维距离数据获取三维点云信息，并发布三维点云话题，所述imu/rtk数据通信节点读取船载imu/rtk组合导航设备测量的imu/rtk数据信息，并发布imu话题与经纬高位置话题，所述三维助航信息通信接点读取ais、ecdis以及radar航行设备测量的航行信息，并发布三维坐标组话题，所述相机数据通信节点读取相机实时视频流参数信息和相机内参数信息，并发布rgb图像话题与相机内参话题；
8.步骤s2：所述相机外参估计模块用于初始化世界系原点，并融合所述三维点云话
题的三维距离数据与所述imu话题和经纬高位置话题的imu/rtk数据信息获取估计外参信息，并发布世界系原点话题与估计外参话题；
9.步骤s3：所述ar助航信息显示模块包括空间坐标系变换节点与ar助航信息渲染显示接点，所述空间坐标系变换节点订阅所述三维坐标组话题、世界系原点话题以及估计外参话题分析获得渲染三维坐标组，并发布渲染三维坐标组话题；
10.步骤s4：所述ar助航信息渲染显示接点订阅rgb图像话题、相机内参话题以及渲染三维坐标组话题完成ar助航信息的显示。
11.进一步的，步骤s1中所述imu/rtk数据信息包括imu/rtk组合导航设备的三轴加速度矢量信息、三轴角速度矢量信息、姿态四元数信息以及经纬高位置信息，所述imu/rtk数据通信节点将所述三轴加速度矢量信息、三轴角速度矢量信息以及姿态四元数信息整合获取imu消息，并发布imu话题，所述imu/rtk数据通信节点基于所述经纬高位置信息发布经纬高位置话题；
12.所述三维助航信息通信接点用于读取ais、ecdis以及radar航行设备发送的符合nmea 0183协议的航行信息，并通过nmea 0183协议解析所述符合航行信息中的三维地理坐标信息，所述三维地理坐标信息基于ros标准三维坐标组消息为基础定义发布三维坐标组话题；
13.所述相机数据通信节点用于读取相机实时视频流参数和相机内参数，所述相机实时视频流参数基于ros标准图像消息为内容发布rgb图像话题，所述ros标准图像消息为ros中sensor_msgs功能包下的image消息，所述相机内参数基于ros标准相机内参消息为内容发布相机内参话题，所述ros标准相机内参消息指的是ros中sensor_msgs功能包下的camerainfo消息。
14.进一步的，步骤s2中所述发布世界系原点话题与估计外参话题具体为：
15.步骤s2.1：所述多传感器数据通信模块发布的经纬高位置话题基于所述eskf融合相机外参估计节点，通过初始时刻经纬高位置初始化世界系原点，并以ros标准三维坐标消息发布世界系原点话题；
16.步骤s2.2：所述三维点云话题与imu话题数据基于所述eskf融合相机外参估计节点获得估计相机外参，且所述eskf融合相机外参估计节点发布以ros标准里程计消息为内容的相机外参话题。
17.进一步的，所述发布世界系原点话题具体为：
18.步骤s2.1.1：所述相机外参估计模块中，相机传感器、imu/rtk传感器以及lidar传感器均固定连接在船体上，且所述相机传感器、imu/rtk传感器以及lidar传感器任意两者之间的空间变换矩阵可通过物理安装参数或联合标定获得，根据船舶上gps天线位置为原点构建船舶固定坐标系o，船艏方向为ox轴，右舷方向为oy轴，垂直向下为oz轴；
19.若已知所述imu/rtk传感器安装位置与该原点o的相对平移为向量t
oi
，相对旋转为矩阵r
oi
，则所述imu/rtk传感器获得的imu/rtk坐标系相对于船舶固定坐标系o的物理安装参数可以通过空间变换矩阵t
oi
来描述：
[0020][0021]
若所述lidar传感器、相机传感器均已知该物理安装参数t
ol
和t
oc
，其中l,c分别代
表lidar坐标系与相机坐标系，则相机传感器与imu/rtk传感器之间的空间变换矩阵t
ci
、相机传感器与lidar传感器之间的空间变换矩阵t
cl
均可以通过矩阵运算获得：
[0022]
t
ci
＝t
co
·
t
oi
[0023]
t
cl
＝t
co
·
t
ol
[0024]
若所述imu/rtk传感器、lidar传感器、相机传感器相对于船舶固定坐标系o的物理安装参数未知，则通过第一联合标定工具，直接获取相机传感器与imu/rtk传感器之间的空间变换矩阵t
ci
，利用第二联合标定工具，直接获取相机传感器与lidar传感器之间的空间变换矩阵t
cl
；所述imu/rtk传感器、lidar传感器与相机传感器的外参通过所述空间变换矩阵进行转换：
[0025]
t
cw
＝t
cl
·
t
lw
＝t
ci
·
t
iw
[0026]
其中，t
cw
表示世界坐标系w到相机坐标系c的空间变换矩阵，t
cl
表示雷达坐标系l到相机坐标系c的空间变换矩阵，t
lw
表示世界坐标系w到雷达坐标系l的空间变换矩阵，t
ci
表示imu坐标系i到相机坐标系c的空间变换矩阵，t
iw
表示世界坐标系w到imu坐标系i的空间变换矩阵；
[0027]
步骤s2.1.2：所述eskf融合相机外参估计节点接收所述三维点云话题、imu话题、经纬高位置话题，利用初始时刻的经纬高位置进行世界坐标系的初始化，即以所述初始时刻的经度、纬度、高度作为东北天世界坐标系w的原点，所述经纬高位置话题与所述东北天世界坐标系w的原点进行地理位置换算；
[0028]
步骤s2.1.3：所述地理位置换算利用库函数reset和初始时刻的经度、纬度、高度设置东北天世界坐标系enu的原点，利用库函数forward得到相对于原点坐标的米制坐标，且以ros标准三维向量消息格式对初始时刻的经、纬、高进行编码，并发布世界系原点话题。
[0029]
进一步的，所述发布相机外参话题具体为：
[0030]
步骤s2.2.1：所述eskf融合相机外参估计节点将待估计的系统状态分为真实状态名义状态误差状态δx，且有关系如下：
[0031][0032]
步骤s2.2.2：为估计相机外参构建以imu姿态矩阵r、速度向量v、位移向量p以及加速度偏差向量ba和角速度偏差向量bg为参数的连续时间下误差状态δx的预测模型
[0033][0034]
其中，f
t
为连续时间下的线性状态转移矩阵，b
t
为连续时间下的测量噪声矩阵，w为测量噪声；
[0035]
步骤s2.2.3：基于所述经纬高位置话题和lidar点云话题获得姿态构建误差状态观测模型：
[0036]
y＝g
t
·
δx+c
t
·n[0037]
其中，g
t
为连续时间下的线性观测矩阵，c
t
为连续时间下的观测噪声矩阵，n为观测噪声，y为连续时间下的误差状态观测量，由经纬高位置话题转换得到的米制坐标提供位移误差观测量δp，由lidar点云匹配提供姿态误差观测量的李代数δθ，其中lidar点云匹配采用变体算法计算当前观测时刻相对于上一观测时刻的姿态变化：
[0038][0039]
其中n
δp
和n
δθ
分别表示位移和姿态的观测误差，i3表示3维的单位矩阵；
[0040]
步骤s2.2.4：以状态递推周期t对eskf预测模型与观测模型y进行离散化处理获取离散化模型：
[0041]
δxk＝f
k-1
δx
k-1
+b
k-1
wk[0042]fk-1
＝i
15
+f
t
·
t，
[0043]
yk＝gkδxk+cknk[0044]
其中，δxk,δx
k-1
表示离散时间下k,k-1时刻递推的误差状态，f
k-1
为离散时间下k-1时刻的线性状态转移矩阵，b
k-1
为离散时间下k-1时刻的测量噪声矩阵，wk为离散时间下k时刻测量噪声，i
15
表示15维的单位矩阵，r
k-1
表示离散时间下k-1时刻的姿态矩阵，yk为离散时间下k时刻的误差状态观测量，gk为离散时间下k时刻的线性观测矩阵，ck为离散时间下k时刻的观测噪声矩阵，nk为离散时间下k时刻的观测噪声；
[0045]
步骤s2.2.5：基于上述离散化模型对误差状态及其协方差进行递推和修正：
[0046][0047]
其中，为k时刻的预测误差状态，为k-1时刻的后验误差状态，为k时刻的预测误差状态协方差，为k-1时刻的后验误差状态协方差，中的上标t表示矩阵的转置，qk为k时刻的测量噪声的协方差，kk为k时刻的卡尔曼增益，rk为k时刻的观测噪声的协方差，为经过修正的后验误差状态，为经过修正的后验误差状态协方差，i为与维度一致的单位矩阵，在每次获得观测量yk的时刻，计算后验误差状态
[0048]
根据中值积分进行名义状态的推算：
[0049]
[0050]
其中，表示k-1时刻的后验姿态矩阵，表示k时刻的预测姿态矩阵，φ为k-1到k时刻的三维旋转向量，符号(
·
)
∧
表示反对称矩阵，ωk,ω
k-1
表示离散时间下k,k-1时刻的角速度，表示离散时间下k,k-1时刻的角速度的偏差，δt为时间间隔，表示k-1时刻的后验速度向量，表示k时刻的预测速度向量，ak,a
k-1
表示离散时间下k,k-1时刻的加速度，表示离散时间下k,k-1时刻的加速度的偏差，g为当地重力加速度，表示k-1时刻的后验位移向量，表示k时刻的预测位移向量；
[0051]
根据真实状态、名义状态以及误差状态的叠加关系，对名义状态进行修正，得到当前时刻的真实状态
[0052][0053]
其中，所有物理量均为k时刻的量，为名义位移向量，为位移向量误差的后验修正值，为真实位移向量，为名义速度向量，为速度向量误差的后验修正值，为真实速度向量，为名义姿态矩阵，为旋转向量误差的后验修正值的反对称矩阵，为真实旋转矩阵，为名义加速度偏差，为加速度偏差误差的后验修正值，为真实加速度偏差，为名义角速度偏差，为角速度偏差误差的后验修正值，为真实角速度偏差；
[0054]
所述真实位移向量与真实姿态矩阵共同构成了k时刻下的相机外参矩阵tk：
[0055][0056]
其中，t
cw,k
为k时刻相机坐标系c相对于enu世界系w的变换矩阵；
[0057]
步骤s2.2.6：所述相机坐标系c以ros标准里程计消息格式nav_msgs/odometry对t
cw,k
进行编码，发布相机外参话题。
[0058]
进一步的，所述步骤s3中发布渲染三维坐标组话题具体为：
[0059]
步骤s3.1：对于三维坐标组话题中的任意几何顶点数据坐标pv,利用开源地理计算库geographiclib中的库函数forward得到该顶点数据坐标pv在世界系w下的米制坐标pw，其中xw,yw,zw表示e、n、u三个方向上的米制坐标，利用相机外参话题数据中相机系c相对于enu世界系w的变换矩阵t
cw
,对几何顶点数据进行三维坐标变换：
[0060][0061]
其中，r
cw
表示相机系c相对于enu世界系e的旋转矩阵，t
cw
表示相机系c相对于enu世界系w的位移向量，0
t
表示三维零向量的转置，对三维坐标组话题中的所有坐标数据应用三维空间变换t
cw
，得到渲染三维坐标组，pc表示相机坐标系下的渲染三维坐标组中的任意几何顶点数据；
[0062]
步骤s3.2：基于三维坐标组同类型消息格式对渲染三维坐标组中几何顶点数据的类别进行编码，并发布渲染三维坐标组话题。
[0063]
进一步的，所述步骤s4中ar助航信息的显示具体为：
[0064]
步骤s4.1：所述ar助航信息渲染显示节点读取所述相机内参话题中的相机内参数f
x
,fy,c
x
,cy,ki,pi，其中f
x
,fy为相机系c下任意的三维空间坐标点到像素平面的缩放变换系数，c
x
,cy为相机系下空间点到像素平面的平移变换系数；ki表示径向畸变修正参数，pi表示切向畸变修正参数；
[0065]
步骤s4.2：所述ar助航信息渲染显示节点读取所述rgb图像话题中的当前帧图像，并获取该帧图像的高度h和宽度w；
[0066]
步骤s4.3：利用图像处理技术，根据所述径向畸变修正参数ki和切向畸变修正参数pi对原始rgb图像进行畸变矫正；
[0067]
步骤s4.4：利用所述相机内参数f
x
,fy,c
x
,cy、所述图像的高度h和宽度w，以及三维渲染引擎中投影视锥体的近裁剪面zn和远裁剪面zf构造符合相机内参数和三维图形引擎的渲染投影矩阵k：
[0068][0069]
步骤s4.5：渲染三维坐标组话题数据中的任意几何顶点数据pc，利用投影矩阵k将待渲染的三维几何坐标pc变换至图像平面坐标系下的像素点pi：
[0070]
pi＝k
·
pc[0071]
步骤s4.6：根据坐标组类别选择预定义的几何绘制方式获取像素坐标组，并将所述像素坐标组叠加至矫正后的rgb图像上，完成ar助航信息的渲染显示。
[0072]
本发明的有益效果：
[0073]
本发明提供了一种智能船舶增强现实助航信息显示方法，通过在ros计算图下的多传感器数据通信模块用于船载lidar、相机以及imu/rtk传感器数据通信及发布话题，在ros计算图下的相机外参估计模块用于通过eskf(误差状态卡尔曼滤波)方法融合lidar传感器和imu/rtk传感器的测量数据以准确估计相机外参及发布话题，在ros计算图下的ar助航信息显示模块,用于将三维ar助航信息转换至相机坐标系，并通过虚实相机匹配完成ar助航信息的精确渲染显示。解决了现有技术中相机外参估计方法较单一、精度较低，多传感器之间数据通信繁杂，以及助航信息显示中渲染数据与真实相机参数不对应的问题。
附图说明
[0074]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0075]
图1为本发明一种智能船舶增强现实助航信息显示方法流程图；
[0076]
图2为本发明一种智能船舶增强现实助航信息显示系统框图。
具体实施方式
[0077]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0078]
本实施例提供了一种智能船舶增强现实助航信息显示方法，如图1和图2所示，包括基于ros(robot operation system)计算图的多传感器数据通信模块、相机外参估计模块以及ar助航信息显示模块；所述多传感器数据通信模块包括lidar(激光雷达)数据通信接点、imu/rtk(inertial measurement unit/real—time kinematic)数据通信节点、三维助航信息通信接点以及相机数据通信节点，包括以下步骤：
[0079]
步骤s1：所述lidar数据通信接点读取船载lidar测量的三维距离数据获取三维点云信息，并发布以ros标准三维点云消息sensor_msgs/pointcloud2为内容的三维点云话题，所述imu/rtk数据通信节点读取船载imu/rtk组合导航设备测量的imu/rtk数据信息，并发布imu话题与经纬高位置话题，所述三维助航信息通信接点读取ais(船舶报告系统)、ecdis(电子海图)以及radar(雷达)航行设备测量的航行信息，并发布三维坐标组话题，所述相机数据通信节点读取相机实时视频流参数信息和相机内参数信息，并发布rgb图像话题与相机内参话题；
[0080]
步骤s2：所述相机外参估计模块用于初始化世界系原点，并融合所述船载lidar测量的三维距离数据与船载imu/rtk组合导航设备测量的imu/rtk数据信息获取估计外参信息，并发布世界系原点话题与估计外参话题；
[0081]
步骤s3：所述ar助航信息显示模块包括空间坐标系变换节点与ar助航信息渲染显示接点，所述空间坐标系变换节点订阅所述三维坐标组话题、世界系原点话题以及估计外参话题分析获得渲染三维坐标组，并发布渲染三维坐标组话题；
[0082]
步骤s4：所述ar助航信息渲染显示接点订阅rgb图像话题、相机内参话题以及渲染三维坐标组话题完成ar助航信息的显示。
[0083]
基于ros(robot operation system)计算图构建了整个ar助航框架，解决了相机、激光雷达、imu、rtk等传感器类型较多，数据通信繁杂的问题，其中多传感器通信模块中的ros节点发布ros标准消息格式的话题数据，在更换不同型号的传感器之后不影响整个ar助航框架，保证了硬件部署、升级的便捷性，该方法中各个模块之间仅有ros话题数据进行通信，模块之间功能解耦，提升了算法部署、升级的便捷性。基于eskf(误差状态卡尔曼滤波)方法将lidar这一高精度距离传感器和imu/rtk的测量数据进行融合，融合了imu短时间内
测量精度较高、lidar感知距离能力强且测量数据漂移小的优势，提升了相机外参估计的精度和准确性，相较于现有的利用船载gps和姿态仪构造相机外参的方法，本发明所提出的基于eskf的方法考虑了传感器误差、测量及观测噪声，并在实时计算中保持对误差和噪声项的估计，避免了长时间运行所带来的累计误差问题，且eskf仅对外参的误差状态进行估计，能够大幅降低运算量，保证ar助航信息渲染的实时性。为保证ar助航信息渲染过程中的投影关系与真实相机一致，本发明提出了同时符合相机内参数和三维图形引擎的渲染投影矩阵，在所述ar助航信息渲染显示节点中利用该矩阵进行投影变换，保证了助航信息渲染的一致性，解决了现有技术实现的ar助航系统存在的虚拟物标和真实物标无法完全一致的现象。
[0084]
在具体实施例中，步骤s1中所述imu/rtk数据信息包括imu/rtk组合导航设备的三轴加速度矢量信息、三轴角速度矢量信息、姿态四元数信息以及经纬高位置信息，所述imu/rtk数据通信节点将所述三轴加速度矢量信息、三轴角速度矢量信息以及姿态四元数信息打包后发布以ros标准imu消息sensor_msgs/imu为内容的imu话题，所述imu/rtk数据通信节点基于所述经纬高位置信息发布以ros标准gnss定位消息sensor_msgs/navsatfix为内容的经纬高位置话题；
[0085]
所述三维助航信息通信接点用于读取ais、ecdis以及radar航行设备发送的符合nmea 0183协议的航行信息，并通过nmea 0183协议解析所述符合航行信息中的三维地理坐标信息，所述三维地理坐标信息基于ros标准三维坐标组消息为基础定义发布以ros标准三维坐标消息geometry_msgs/vector3stamped为基础定义包含助航信息类别的三维坐标组消息，并发布以该消息为内容的三维坐标组话题；
[0086]
所述相机数据通信节点用于读取相机实时视频流参数和相机内参数，所述相机实时视频流参数基于ros标准图像消息为内容发布rgb图像话题，所述ros标准图像消息为ros中sensor_msgs功能包下的image消息，所述相机内参数基于ros标准相机内参消息为内容发布相机内参话题，所述ros标准相机内参消息指的是ros中sensor_msgs功能包下的camerainfo消息。
[0087]
在具体实施例中，步骤s2中所述发布世界系原点话题与估计外参话题具体为：
[0088]
步骤s2.1：所述多传感器数据通信模块发布的经纬高位置话题基于所述eskf融合相机外参估计节点，通过初始时刻经纬高位置初始化世界系原点，并以ros标准三维坐标消息geometry_msgs/vector3stamped发布世界系原点话题；
[0089]
步骤s2.2：所述三维点云话题与imu话题数据基于所述eskf融合相机外参估计节点获得估计相机外参，且所述eskf融合相机外参估计节点发布以ros标准里程计nav_msgs/odometry消息为内容的相机外参话题。
[0090]
在具体实施例中，所述发布世界系原点话题具体为：
[0091]
步骤s2.1.1：所述相机外参估计模块中，相机传感器、imu/rtk传感器以及lidar传感器均固定连接在船体上，且所述相机传感器、imu/rtk传感器以及lidar传感器任意两者之间的空间变换矩阵可通过物理安装参数或联合标定获得，根据船舶上gps天线位置为原点构建船舶固定坐标系o，船艏方向为ox轴，右舷方向为oy轴，垂直向下为oz轴；
[0092]
若已知所述imu/rtk传感器安装位置与该原点o的相对平移为向量t
oi
，相对旋转为矩阵r
oi
，则所述imu/rtk传感器获得的imu/rtk坐标系相对于船舶固定坐标系o的物理安装
参数可以通过空间变换矩阵t
oi
来描述：
[0093][0094]
若所述lidar传感器、相机传感器均已知该物理安装参数t
ol
和t
oc
，其中l,c分别代表lidar坐标系与相机坐标系，则相机传感器与imu/rtk传感器之间的空间变换矩阵t
ci
、相机传感器与lidar传感器之间的空间变换矩阵t
cl
均可以通过矩阵运算获得：
[0095]
t
ci
＝t
co
·
t
oi
[0096]
t
cl
＝t
co
·
t
ol
[0097]
若所述imu/rtk传感器、lidar传感器、相机传感器相对于船舶固定坐标系o的物理安装参数未知，则通过第一联合标定工具，直接获取相机传感器与imu/rtk传感器之间的空间变换矩阵t
ci
，所述第一联合标定工具为kalibr相机-imu联合标定工具；利用第二联合标定工具，直接获取相机传感器与lidar传感器之间的空间变换矩阵t
cl
，所述第二联合标定工具为autoware相机-lidar联合标定工具；
[0098]
所述imu/rtk传感器、lidar传感器与相机传感器的外参通过所述空间变换矩阵进行转换：
[0099]
t
cw
＝t
cl
·
t
lw
＝t
ci
·
t
iw
[0100]
其中，t
cw
表示世界坐标系w到相机坐标系c的空间变换矩阵，t
cl
表示雷达坐标系l到相机坐标系c的空间变换矩阵，t
lw
表示世界坐标系w到雷达坐标系l的空间变换矩阵，t
ci
表示imu坐标系i到相机坐标系c的空间变换矩阵，t
iw
表示世界坐标系w到imu坐标系i的空间变换矩阵；
[0101]
步骤s2.1.2：所述eskf融合相机外参估计节点接收所述三维点云话题、imu话题、经纬高位置话题，利用初始时刻的经纬高位置进行世界坐标系的初始化，即以所述初始时刻的经度、纬度、高度作为东北天世界坐标系w的原点，所述经纬高位置话题与所述东北天世界坐标系w的原点利用开源地理计算库geographiclib进行地理位置换算；
[0102]
步骤s2.1.3：所述地理位置换算利用库函数reset(经、纬、高)和初始时刻的经度、纬度、高度设置东北天世界坐标系enu的原点，利用库函数forward(经、纬、高、e、n、u)得到相对于原点经、纬、高的米制坐标e、n、u，且以ros标准三维向量消息格式geometry_msgs/vector3stamped对初始时刻的经、纬、高进行编码，并发布世界系原点话题。
[0103]
在具体实施例中，所述发布相机外参话题具体为：
[0104]
步骤s2.2.1：所述eskf融合相机外参估计节点将待估计的系统状态分为真实状态名义状态误差状态δx，且有关系如下：
[0105][0106]
步骤s2.2.2：为估计相机外参构建以imu姿态矩阵r、速度向量v、位移向量p以及加速度偏差向量ba和角速度偏差向量bg为参数的连续时间下误差状态δx的预测模型
[0107][0108]
其中，f
t
为连续时间下的线性状态转移矩阵，b
t
为连续时间下的测量噪声矩阵，w为测量噪声；
[0109]
步骤s2.2.3：基于所述经纬高位置话题和lidar点云话题获得姿态构建误差状态
观测模型：
[0110]
y＝g
t
·
δx+c
t
·n[0111]
其中，g
t
为连续时间下的线性观测矩阵，c
t
为连续时间下的观测噪声矩阵，n为观测噪声，y为连续时间下的误差状态观测量，由经纬高位置话题转换得到的米制坐标提供位移误差观测量δp，由lidar点云匹配提供姿态误差观测量的李代数δθ，其中lidar点云匹配采用icp、ndt或gicp、fast-vgicp、ptplicp、ndt-omp中的任意一种变体算法计算当前观测时刻相对于上一观测时刻的姿态变化：
[0112][0113]
其中n
δp
和n
δθ
分别表示位移和姿态的观测误差，i3表示3维的单位矩阵；
[0114]
步骤s2.2.4：以状态递推周期t对eskf预测模型与观测模型y进行离散化处理获取离散化模型：
[0115]
δxk＝f
k-1
δx
k-1
+b
k-l
wk[0116]fk-1
＝i
15
+f
t
·
t，
[0117]
yk＝gkδxk+cknk[0118]
其中，δxk,δx
k-1
表示离散时间下k,k-1时刻递推的误差状态，f
k-1
为离散时间下k-1时刻的线性状态转移矩阵，b
k-1
为离散时间下k-1时刻的测量噪声矩阵，wk为离散时间下k时刻测量噪声，i
15
表示15维的单位矩阵，r
k-1
表示离散时间下k-1时刻的姿态矩阵，yk为离散时间下k时刻的误差状态观测量，gk为离散时间下k时刻的线性观测矩阵，ck为离散时间下k时刻的观测噪声矩阵，nk为离散时间下k时刻的观测噪声；
[0119]
步骤s2.2.5：基于上述离散化模型对误差状态及其协方差进行递推和修正：
[0120][0121]
其中，为k时刻的预测误差状态，为k-1时刻的后验误差状态，为k时刻的预测误差状态协方差，为k-1时刻的后验误差状态协方差，中的上标t表示矩阵的转置，qk为k时刻的测量噪声的协方差，kk为k时刻的卡尔曼增益，rk为k时刻的观测噪声的协方差，为经过修正的后验误差状态，为经过修正的后验误差状态协方差，i为与维度一致的单位矩阵，在每次获得观测量yk的时刻，计算后验误差状态
[0122]
根据中值积分进行名义状态的推算：
[0123][0124]
其中，表示k-1时刻的后验姿态矩阵，表示k时刻的预测姿态矩阵，φ为k-1到k时刻的三维旋转向量，符号(
·
)
∧
表示反对称矩阵，ωk,ω
k-1
表示离散时间下k,k-1时刻的角速度，表示离散时间下k,k-1时刻的角速度的偏差，δt为时间间隔，表示k-1时刻的后验速度向量，表示k时刻的预测速度向量，ak,a
k-1
表示离散时间下k,k-1时刻的加速度，表示离散时间下k,k-1时刻的加速度的偏差，g为当地重力加速度，表示k-1时刻的后验位移向量，表示k时刻的预测位移向量；
[0125]
根据真实状态、名义状态以及误差状态的叠加关系，对名义状态进行修正，得到当前时刻的真实状态
[0126][0127]
其中，所有物理量均为k时刻的量，为名义位移向量，为位移向量误差的后验修正值，为真实位移向量，为名义速度向量，为速度向量误差的后验修正值，为真实速度向量，为名义姿态矩阵，为旋转向量误差的后验修正值的反对称矩阵，为真实旋转矩阵，为名义加速度偏差，为加速度偏差误差的后验修正值，为真实加速度偏差，为名义角速度偏差，为角速度偏差误差的后验修正值，为真实角速度偏差；
[0128]
所述真实位移向量与真实姿态矩阵共同构成了k时刻下的相机外参矩阵tk：
[0129][0130]
其中，t
cw,k
为k时刻相机坐标系c相对于enu世界系w的变换矩阵；
[0131]
步骤s2.2.6：所述相机坐标系c以ros标准里程计消息格式nav_msgs/odometry对
t
cw,k
进行编码，发布相机外参话题。
[0132]
在具体实施例中，所述步骤s3中发布渲染三维坐标组话题具体为：
[0133]
步骤s3.1：对于三维坐标组话题中的任意几何顶点数据坐标pv,其中包含了经、纬、高信息，利用开源地理计算库geographiclib中的库函数forward得到该顶点数据坐标pv在世界系w下的米制坐标pw，其中xw,yw,zw表示e、n、u三个方向上的米制坐标，利用相机外参话题数据中相机系c相对于enu世界系w的变换矩阵t
cw
,对几何顶点数据进行三维坐标变换：
[0134][0135]
其中，r
cw
表示相机系c相对于enu世界系w的旋转矩阵，t
cw
表示相机系c相对于enu世界系w的位移向量，0
t
表示三维零向量的转置，对三维坐标组话题中的所有坐标数据应用三维空间变换t
cw
，得到渲染三维坐标组，pc表示相机坐标系下的渲染三维坐标组中的任意几何顶点数据；
[0136]
步骤s3.2：基于三维坐标组同类型消息格式对渲染三维坐标组中几何顶点数据的类别进行编码，并发布渲染三维坐标组话题。
[0137]
在具体实施例中，所述步骤s4中ar助航信息的显示具体为：
[0138]
步骤s4.1：所述ar助航信息渲染显示节点读取所述相机内参话题中的相机内参数f
x
,fy,c
x
,cy,ki,pi，其中f
x
,fy为相机系c下任意的三维空间坐标点到像素平面的缩放变换系数，c
x
,cy为相机系下空间点到像素平面的平移变换系数；ki表示径向畸变修正参数，pi表示切向畸变修正参数；
[0139]
步骤s4.2：所述ar助航信息渲染显示节点读取所述rgb图像话题中的当前帧图像，并获取该帧图像的高度h和宽度w；
[0140]
步骤s4.3：利用图像处理技术，根据所述径向畸变修正参数ki和切向畸变修正参数pi对原始rgb图像进行畸变矫正，所述畸变矫正为现有公知技术，并非本技术的发明点，在此不在赘述；
[0141]
步骤s4.4：利用所述相机内参数f
x
,fy,c
x
,cy、所述图像的高度h和宽度w，以及三维渲染引擎中投影视锥体的近裁剪面zn和远裁剪面zf构造符合相机内参数和三维图形引擎的渲染投影矩阵k：
[0142][0143]
步骤s4.5：渲染三维坐标组话题数据中的任意几何顶点数据pc，利用投影矩阵k将待渲染的三维几何坐标pc变换至图像平面坐标系下的像素点pi：
[0144]
pi＝k
·
pc[0145]
步骤s4.6：根据坐标组类别选择预定义的几何绘制方式获取像素坐标组，并将所述像素坐标组叠加至矫正后的rgb图像上，完成ar助航信息的渲染显示。
[0146]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。