一种无人系统多源信息融合定位系统

1.本发明涉及传感器信息融合领域，具体涉及一种无人系统多源信息融合定位系统。
背景技术：
：：2.无人系统的精准定位是无人系统能够顺利运行的重要前提条件。常见的定位方法有全球导航卫星系统(gnss)定位、轮式里程计定位、惯性导航系统、激光雷达(lidar)定位、视觉定位和基于v2x技术的定位。gnss信息通过差分计算后得到高精度位置信息，但gnss信息对周围环境要求较高，容易受到外部干扰。轮式里程计和惯导定位拥有短时间内高精度定位，不依赖外部信息，不受外部环境干扰，能够适应多种工作环境的优点，但惯导系统随着时间积累，精度会随之下降，长期精度较差。因此在无人系统实际使用中，经常使用多种定位方法，将它们进行定位融合，从而获得更精准得定位结果。申请号[cn202110426739.9]的专利提出“基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统”，通过获取gnss信号中的伪距和伪距率的观测值做差获得伪距误差和伪距误差率，获取视觉提供的航向增量与ins获得航向增量做差得到航向误差，根据这些信息采用序贯卡尔曼滤波方法得到误差修正信息，从而提高定位精度，如图1所示。申请号[cn202110082996.5]的专利提出“基于图优化的行人定位方法及系统”，通过将获取的gnss信息、惯导信息和气压高度信息存入数据缓存中，对于缓存中的数据一路使用ekf滤波器进行滤波融合获得实时定位结果，另一路使用历史数据通过图优化方法对ekf滤波器的滤波融合定位结果进行优化修正，从而进一步提高定位精度，如图2所示。[0003]以上专利都使用了多源定位信息融合定位来提高定位精度，但第一种使用卡尔曼滤波进行融合定位方法受限于卡尔曼滤波具有线性化误差大、且忽略了历史约束信息，并在节点灵活增删方面具有局限性，这些问题导致在复杂工作条件下无法获得更精准的定位精度。第二种在使用ekf滤波进行融合定位的同时，也将历史定位信息通过最小二乘位置优化的方法来对ekf滤波融合定位进行修正，解决了ekf滤波方法无法使用历史定位信息的问题，极大的提高了在复杂工作条件下的定位精度。[0004]但上述两种方法都没有判断gnss定位信息的质量。复杂的工作条件下，无人系统常常需要穿梭于建筑物等大型遮挡物之中，当在大型遮挡物附近运动时，遮挡可能会导致gnss信号弱，导致gnss的质量严重下降使得定位精准性大幅降低。这些质量差的gnss定位信息若被使用于融合定位中，将导致无人系统的定位产生严重的偏差。[0005]同时第二种方法没有对惯导信息进行矫正。复杂的工作条件下，由于外界环境的干扰(例如路面太滑导致打滑，路面不平整、倾斜)，里程计与惯导的定位信息常常会出现偏差，如果不及时修正这些偏差，偏差随时间增长会一直增大，导致在优化的过程残差无法收敛，导致最小二乘位置优化失败。技术实现要素：[0006]本发明提供了一种应用于无人系统在复杂工作条件下的无人系统多源信息融合定位系统，旨在解决在复杂工作条件下时，由于gnss信号被干扰、遮挡导致定位信息错误，以及里程计和惯导定位信息随时间增长误差增大的问题，保证在复杂工作条件下，无人系统定位准确性与稳定性。[0007]根据本发明的第一方面，提供一种无人系统多源信息融合定位系统，其特征在于，包括：[0008]gnss定位信息模块，用于获取gnss信号中的headinga信息与gngga信息，并发送给gnss定位融合输入信息处理模块。[0009]gnss定位融合输入信息处理模块，用于gnss信息质量判断，包括：通过时间戳同步headinga信息与gngga信息，并从headinga信息中获取使用卫星数、基线长、解状态、位置类型和航向角6个状态量，通过使用卫星数、基线长、解状态和位置类型来判断该时刻gngga信息的质量，通过gngga信息与航向角状态量组成卫星位姿信息，并保存进缓存队列中。[0010]里程计定位融合输入信息处理模块，用于矫正定位位姿数据的偏移量，将矫正里程计位姿数据作为图优化模型的里程计位姿信息，并且使用下一时刻里程计位姿数据和矫正里程计位姿数据更新里程计位姿与矫正里程计位姿的旋转矩阵，将该矩阵作为里程计矫正旋转矩阵。[0011]定位融合模块，用于融合卫星位姿信息和里程计位姿信息，包括：从缓存队列中获取历史卫星位姿信息和里程计位姿信息，并时间同步两类位姿信息，将同步后的历史卫星位姿信息和里程计位姿信息使用图优化方法进行优化修正，获取到优化位姿信息，通过优化位姿信息和未优化位姿信息更新未优化位姿信息与优化位姿信息的旋转矩阵，将该矩阵作为图优化融合旋转矩阵。[0012]融合定位信息输出模块，用于计算并输出融合定位信息，包括：获取里程计矫正旋转矩阵和图优化融合旋转矩阵，计算二者的乘积获得最终融合定位后的旋转矩阵，将该矩阵作为最终融合结果输出。[0013]进一步地，本发明所提供的无人系统多源信息融合定位系统，其特征在于，gnss定位信息模块还包括：[0014]获取gngga信息中包含的经纬度、gps质量信息、水平精度因子、天线海拔高度；将这些信息填充人ros的sensor_msgs/navsatfix消息sat中；sat.header.stamp代表时间戳存放此刻时间，sat.status.status代表gps质量信息，sat.latitude代表纬度信息，sat.longitude代表经度信息，sat.altitude代表天线海拔高度、sat.position_covariance[0，4]代表水平精度因子，填充好后将sat信息发送给gps定位融合输入信息处理模块。[0015]获取headinga信息(接收速度为1hz)中包含的获取解状态和位置类型、使用卫星数、航向角、基线长，将这些信息填充人ros的geometry_msgs：：quaternionstamped消息qua中，qua.quaternion.x代表解状态和位置类型，x＝1代表解状态为sol_computed且位置类型为narrow_int，其他状态时x＝0，qua.quaternion.y代表使用的卫星数、qua.quaternion.z代表航向角，qua.quaternion.w代表基线长，填充好后将qua信息发送给gps定位融合输入信息处理模块。[0016]进一步地，本发明所提供的无人系统多源信息融合定位系统，其特征在于，gnss定位融合输入信息处理模块还包括：[0017]该模块负责将地球坐标系的位姿数据转换成emu坐标系的位姿数据，并根据卫星数和基线长等信息判断该位姿信息的质量情况，筛选质量优良的位姿数据作为图优化模型的第一定位融合数据。[0018]获取gnss定位信息模块发送的sat信息和qua信息；首先判断sat信息和qua信息的时间戳是否相同，时间戳相同说明sat信息和qua信息是同一时刻的定位信息；获取同一时间的sat信息与qua信息后，创建一个ros的nav_msgs/odometry信息gnssodom；将sat信息的经纬度使用geographiclib类型的forward方法转换成emu坐标系的位置信息并填人gnssodom.pose.pose.position中；将qua信息中的航向角使用tf库的createquaternionfromyaw方法转换成姿态四元素信息并填入gnssodom.pose.pose.orientation中，此时初始化gnssodom.pose.covariance[0]为0.01，代表gnssodom在融合时位置数据的权重，值越小表示权重越大，初始化gnssodom.twist.covariance[0]为0.05，代表gnssodom在融合时姿态数据的权重，值越小表示权重越大。[0019]获取到gnssodom信息后，判断该定位数据的质量是否优良，保证进入融合定位的gnss定位数据是准确的，具体流程如下：[0020](1)判断qua.quaternion.x＜0.8llqua.quaternion.y＜10为true，说明该定位数据质量很差，直接丢弃该gnssodom，并将stable＝0，然后直接退出判断，不再进行其他操作，等待下个sat和qua信息；若为false，则到第2步；stable参数是当gnssodom信息从质量较差恢复到质量好时，要等待gnssodom信息质量良好并保持直到获取下一个gnssodom信息的已等待时间。[0021](2)判断qua.quaternion.y＜15为true，则进人第3步；若为false，则进人第4步。[0022](3)判断0.6＜qua.quaternion.w＜0.65为true，说明此时在卫星数量较少的情况下gnssodom定位精度一般，此时降低gnssodom在融合定位时的权重，将gnssodom.pose.covariance[0]设置为5，gnssodom.twist.covariance[0]设置为0.1，然后进人第4步；若为false，则直接退出，不再进行其他操作，等待下个sat和qua信息。[0023](4)将stable+1，然后进人第5步。[0024](5)判断qua.quaternion.y》18为ture，说明此时gnssodom信息质量很好，将stable+4，以尽快获取gnssodom信息，然后进入第6步；若为false，则直接进入第6步。[0025](6)判断stable＜60为true，则说明gnssodom信息质量良好还未保持一定时间，直接退出判断，不再进行其他操作，等待下个sat和qua信息；若为false，则进入第7步。[0026](7)此刻的gnssodom信息可以作为融合定位图优化模型的卫星位姿信息。[0027]当gnssodom信息质量差时，此时无人系统的定位信息只使用里程计数据进行图优化，去除了gnssodom错误定位信息对无人系统定位准确性的干扰。[0028]进一步地，本发明所提供的无人系统多源信息融合定位系统，其特征在于，里程计定位融合输入信息处理模块还包括：[0029]获取到无人系统底盘发送来的odom信息后，将之标记成curodom，与前一时刻收到的odom信息lastodom进行位置差值运算求得这段时间无人系统的位移数据，计算公式如下：[0030]si+1(x，y，z)＝ai+1(x，y，z)-ai(x，y，z)[0031]ai(x，y，z)代表里程计第i时刻里程计位姿数据，由于里程计定位数据偏移，导致计算得到的si(x，y，z)》实际的位移(x，y，z)，因此缩小si(x，y，z)再将位移值加到第i时刻矫正位姿数据中，就能获得第i+1时刻矫正位姿数据，所以当前时刻优化后的位姿数据计算公式如下[0032]βi+1(x，y，z)＝βi(x，y，z)+(si+1(x，y，z)·scaler)[0033]βi(x，y，z)为里程计第i时刻矫正位姿数据，scaler为矫正参数值(scaler＜1)，通过减小计算的位移量，矫正里程计与惯导融合后的定位数据偏移，得到的βi(x，y，z)转换成位姿数据odom是更精确的里程计定位数据，将odom作为图优化模型的里程计位姿信息，使用第i+1时刻里程计位姿数据和矫正里程计位姿数据更新里程计位姿与矫正里程计位姿的旋转矩阵，将该矩阵作为里程计矫正旋转矩阵：[0034][0035]其中为里程计矫正旋转矩阵，ai-1为第i时刻里程计位姿的逆，bi表示第i时刻矫正里程计位姿矩阵。[0036]进一步地，本发明所提供的无人系统多源信息融合定位系统，其特征在于，定位融合模块还包括：[0037]当缓存队列中的数据量达到20时，获取卫星位姿信息gnssodom和里程计位姿信息odom，比较gnssodom的时间戳gnssodom.header.stamp与odom的时间戳odom.header.stamp是否相等，若相等则将gnssodom与odom送入图优化模型中进行融合定位。[0038]图优化模型中，使用如下公式来优化每时刻的里程计位姿信息：[0039][0040]tt代表t时刻位姿转换到t+1时刻位姿的旋转矩阵，xt代表t时刻位姿与t+1时刻位姿差矩阵，ht代表t时刻位姿所占权重矩阵。[0041]使用如下公式通过每时刻的gps位姿信息来优化里程计位姿信息：[0042][0043][0044]tgt代表t时刻gps的位姿矩阵，xgt代表t时刻里程计的位姿矩阵，hgt代表t时刻gps位姿所占权重矩阵，pi代表损失函数。[0045]通过图优化计算后，得到融合的位姿信息，使用最后一个时刻的里程计位姿和融合的位姿信息来更新旋转矩阵，计算公式如下：[0046][0047]代表里程计位姿矩阵转换成融合的位姿矩阵的旋转矩阵，oα-1为里程计位姿矩阵的逆，optβ表示融合的位姿矩阵。[0048]将作为图优化融合旋转矩阵发送给融合定位信息输出模块。[0049]进一步地，本发明所提供的无人系统多源信息融合定位系统，其特征在于，融合定位信息输出模块还包括：[0050]该模块获取里程计矫正旋转矩阵与图优化融合旋转矩阵计算统一的转换关系矩阵[0051][0052]将作为多源融合定位方法的最终输出融合旋转矩阵。[0053]与现有技术相比，本发明所构思的上述技术方案至少具有以下有益效果：本发明对无人系统不同传感器采集的位置信息按照时间对齐以后，将所有位置数据映射到一个有向图，基于最小二乘原理进行schur分解迭代求取每个数据的误差和最佳估计，从而获得每个时刻的最佳定位估计。进一步的，由于每个传感器的非高斯误差会导致迭代速度慢、计算开销升高、求解快速陷入局部最优状态，本方法还通过先验知识对gnss、里程计和惯性单元、视觉传感器数据进行校验和矫正，使得进入迭代的数据满足高斯分布，使得迭代过程中残差顺利收敛，保证无人系统运行过程中定位稳定性与准确性。[0054]应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明。附图说明[0055]此处的附图被并人说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。[0056]图1是现有技术中基于序贯卡尔曼滤波的gnssins视觉融合定位系统。[0057]图2是现有技术中基于图优化的行人定位系统。[0058]图3是根据一示例性实施例示出的无人系统多源信息融合定位系统模块示意图。[0059]图4是根据一示例性实施例示出的gnss定位信息模块。[0060]图5是根据一示例性实施例示出的gnss定位融合输入信息处理模块。[0061]图6是根据一示例性实施例示出的里程计定位融合输入信息处理模块。[0062]图7是根据一示例性实施例示出的定位融合模块。[0063]图8是根据一示例性实施例示出的融合定位信息输出模块。具体实施方式[0064]为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。[0065]本发明技术的详细技术方案，如图3所示包括：[0066]gnss定位信息模块，用于获取gnss信号中的headinga信息与gngga信息，并发送给gnss定位融合输入信息处理模块。[0067]gnss定位融合输入信息处理模块，用于gnss信息质量判断，包括：通过时间戳同步headinga信息与gngga信息，并从headinga信息中获取使用卫星数、基线长、解状态、位置类型和航向角6个状态量，通过使用卫星数、基线长、解状态和位置类型来判断该时刻gngga信息的质量，通过gngga信息与航向角状态量组成卫星位姿信息，并保存进缓存队列中；[0068]里程计定位融合输入信息处理模块，用于矫正定位位姿数据的偏移量，将矫正里程计位姿数据作为图优化模型的里程计位姿信息，并且使用下一时刻里程计位姿数据和矫正里程计位姿数据更新里程计位姿与矫正里程计位姿的旋转矩阵，将该矩阵作为里程计矫正旋转矩阵；[0069]定位融合模块，用于融合卫星位姿信息和里程计位姿信息，包括：从缓存队列中获取历史卫星位姿信息和里程计位姿信息，并时间同步两类位姿信息，将同步后的历史卫星位姿信息和里程计位姿信息使用图优化方法进行优化修正，获取到优化位姿信息，通过优化位姿信息和未优化位姿信息更新未优化位姿信息与优化位姿信息的旋转矩阵，将该矩阵作为图优化融合旋转矩阵；[0070]融合定位信息输出模块，用于计算并输出融合定位信息，包括：获取里程计矫正旋转矩阵和图优化融合旋转矩阵，计算二者的乘积获得最终融合定位后的旋转矩阵，将该矩阵作为最终融合结果输出。[0071]本发明技术方案首先使用headinga信息筛选质量优良的gngga信息，解决了在复杂工作条件下，gnss信号弱导致gngga错误定位信息被用于融合定位的问题，保证进入融合定位中的gngga信息是gnss信号保持一定时间良好后获取的，消除了gngga定位信息不准确对融合的干扰。通过超参数scaler修正位移距离，实现里程计数据偏移矫正方法，从而解决了在复杂工作条件下，里程计信息随时间增长偏差增大，导致图优化无法收敛的问题通过以上方法，保证了在无人系统在复杂工作条件下的精确定位。[0072]具体地，各个模块内容包括：[0073]gnss定位信息模块如图4所示：[0074]获取gngga信息中包含的经纬度、gps质量信息、水平精度因子、天线海拔高度；将这些信息填充人ros的sensor_msgs/navsatfix消息sat中；sat.header.stamp代表时间戳存放此刻时间，sat.status.status代表gps质量信息，sat.latitude代表纬度信息，sat.longitude代表经度信息，sat.altitude代表天线海拔高度、sat.position_covariance[0，4]代表水平精度因子，填充好后将sat信息发送给gps定位融合输入信息处理模块。[0075]获取headinga信息(接收速度为1hz)中包含的获取解状态和位置类型、使用卫星数、航向角、基线长，将这些信息填充入ros的geometry_msgs：：quatemionstamped消息qua中，qua.quaternion.x代表解状态和位置类型，x＝1代表解状态为sol_computed且位置类型为narrow_int，其他状态时x＝0，qua.quaternion.y代表使用的卫星数、qua.quaternion.z代表航向角，qua.quaternion.w代表基线长，填充好后将qua信息发送给gps定位融合输入信息处理模块。[0076]gnss定位融合输入信息处理模块如图5所示：[0077]该模块负责将地球坐标系的位姿数据转换成emu坐标系的位姿数据，并根据卫星数和基线长等信息判断该位姿信息的质量情况，筛选质量优良的位姿数据作为图优化模型的第一定位融合数据。[0078]获取gnss定位信息模块发送的sat信息和qua信息；首先判断sat信息和qua信息的时间戳是否相同，时间戳相同说明sat信息和qua信息是同一时刻的定位信息；获取同一时间的sat信息与qua信息后，创建一个ros的nav_msgs/odometry信息gnssodom；将sat信息的经纬度使用geographiclib类型的forward方法转换成emu坐标系的位置信息并填人gnssodom.pose.pose.position中；将qua信息中的航向角使用tf库的createquaternionfromyaw方法转换成姿态四元素信息并填人gnssodom.pose.pose.orientation中，此时初始化gnssodom.pose.covariance[0]为0.01，代表gnssodom在融合时位置数据的权重，值越小表示权重越大，初始化gnssodom.twist.covariance[0]为0.05，代表gnssodom在融合时姿态数据的权重，值越小表示权重越大。[0079]获取到gnssodom信息后，判断该定位数据的质量是否优良，保证进入融合定位的gnss定位数据是准确的，具体流程如下：[0080](1)判断qua.quaternion.x＜0.8||qua.quaternion.y＜10为true，说明该定位数据质量很差，直接丢弃该gnssodom，并将stable＝0，然后直接退出判断，不再进行其他操作，等待下个sat和qua信息；若为false，则到第2步；stable参数是当gnssodom信息从质量较差恢复到质量好时，要等待gnssodom信息质量良好并保持直到获取下一个gnssodom信息的已等待时间。[0081](2)判断qua.quaternion.y＜15为true，则进入第3步；若为false，则进人第4步。[0082](3)判断0.6＜qua.quaternion.w＜0.65为true，说明此时在卫星数量较少的情况下gnssodom定位精度一般，此时降低gnssodom在融合定位时的权重，将gnssodom.pose.covariance[0]设置为5，gnssodom.twist.covariance[0]设置为0.1，然后进入第4步；若为false，则直接退出，不再进行其他操作，等待下个sat和qua信息。[0083](4)将stable+1，然后进入第5步。[0084](5)判断qua.quaternion.y》18为ture，说明此时gnssodom信息质量很好，将stable+4，以尽快获取gnssodom信息，然后进入第6步；若为false，则直接进人第6步。[0085](6)判断stable＜60为true，则说明gnssodom信息质量良好还未保持一定时间，直接退出判断，不再进行其他操作，等待下个sat和qua信息；若为false，则进入第7步。[0086](7)此刻的gnssodom信息可以作为融合定位图优化模型的卫星位姿信息。[0087]当gnssodom信息质量差时，此时无人系统的定位信息只使用里程计数据进行图优化，去除了gnssodom错误定位信息对无人系统定位准确性的干扰。[0088]里程计定位融合输入信息处理模块如图6所示：[0089]该模块负责矫正从无人系统底盘获取的里程计与惯导融合后的定位数据。由于由于外界环境的干扰导致该定位数据出现偏移，该模块负责矫正定位位姿数据的偏移量，将矫正里程计位姿数据作为图优化模型的里程计位姿信息，并且使用第i+1时刻里程计位姿数据和矫正里程计位姿数据更新里程计位姿与矫正里程计位姿的旋转矩阵，将该矩阵作为里程计矫正旋转矩阵。[0090]获取到无人系统底盘发送来的odom信息后，将之标记成curodom，与前一时刻收到的odom信息lastodom进行位置差值运算求得这段时间无人系统的位移数据，计算公式如下：[0091]si+1(x，y，z)＝ai+1(x，y，z)-ai(x，y，z)[0092]ai(x，y，z)代表里程计第i时刻里程计位姿数据，由于里程计定位数据偏移，导致计算得到的si(x，y，z)》实际的位移(x，y，z)，因此缩小si(x，y，z)再将位移值加到第i时刻矫正位姿数据中，就能获得第i+1时刻矫正位姿数据，所以当前时刻优化后的位姿数据计算公式如下[0093]βi+1(x，y，z)＝βi(x，y，z)+(si+1(x，y，z)·scaler)[0094]βi(x，y，z)为里程计第i时刻矫正位姿数据，scaler为矫正参数值(scaler＜1)，通过减小计算的位移量，矫正里程计与惯导融合后的定位数据偏移，得到的βi(x，y，z)转换成位姿数据odom是更精确的里程计定位数据，将odom作为图优化模型的里程计位姿信息，使用第i+1时刻里程计位姿数据和矫正里程计位姿数据更新里程计位姿与矫正里程计位姿的旋转矩阵，将该矩阵作为里程计矫正旋转矩阵：[0095][0096]其中为里程计矫正旋转矩阵，ai-1为第i时刻里程计位姿的逆，bi表示第i时刻矫正里程计位姿矩阵。[0097]定位融合模块如图7所示：[0098]当缓存队列中的数据量达到20时，获取卫星位姿信息gnssodom和里程计位姿信息odom，比较gnssodom的时间戳gnssodom.header.stamp与odom的时间戳odom.header.stamp是否相等，若相等则将gnssodom与odom送人图优化模型中进行融合定位。[0099]图优化模型中，使用如下公式来优化每时刻的里程计位姿信息：[0100][0101]tt代表t时刻位姿转换到t+1时刻位姿的旋转矩阵，xt代表t时刻位姿与t+1时刻位姿差矩阵，ht代表t时刻位姿所占权重矩阵。[0102]使用如下公式通过每时刻的gps位姿信息来优化里程计位姿信息：[0103][0104][0105]tgt代表t时刻gps的位姿矩阵，xgt代表t时刻里程计的位姿矩阵，hgt代表t时刻gps位姿所占权重矩阵，pi代表损失函数。[0106]通过图优化计算后，得到融合的位姿信息，使用最后一个时刻的里程计位姿和融合的位姿信息来更新旋转矩阵，计算公式如下：[0107][0108]代表里程计位姿矩阵转换成融合的位姿矩阵的旋转矩阵，oα-1为里程计位姿矩阵的逆，optβ表示融合的位姿矩阵。[0109]将作为图优化融合旋转矩阵发送给融合定位信息输出模块。[0110]融合定位信息输出模块如图8所示：[0111]融合定位信息输出模块获取里程计矫正旋转矩阵和图优化融合旋转矩阵。计算图优化融合旋转矩阵*里程计矫正旋转矩阵，获得最终融合定位后的旋转矩阵。将两个旋转矩阵进行整合，获得最最终融合输出矩阵。从而使得融合输出矩阵即优化了rtk质量差，又优化了里程计偏移这两个问题的转换关系矩阵。[0112]该模块获取里程计矫正旋转矩阵与图优化融合旋转矩阵计算统一的转换关系矩阵[0113][0114]将作为多源融合定位方法的最终输出融合旋转矩阵。[0115]领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本发明的其它实施方案。本技术旨在涵盖本发明的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本发明的一般性原理并包括本公开未公开的本
技术领域：
：中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本发明的真正范围和精神由下面的权利要求指出。[0116]应当理解的是，本发明并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本发明的范围仅由所附的权利要求来限制。当前第1页12当前第1页12
技术特征：
1.一种无人系统多源信息融合定位系统，其特征在于，包括：gnss定位信息模块，用于获取gnss信号中的headinga信息与gngga信息，并发送给gnss定位融合输入信息处理模块；gnss定位融合输入信息处理模块，用于gnss信息质量判断，包括：通过时间戳同步headinga信息与gngga信息，并从headinga信息中获取使用卫星数、基线长、解状态、位置类型和航向角6个状态量，通过使用卫星数、基线长、解状态和位置类型来判断该时刻gngga信息的质量，通过gngga信息与航向角状态量组成卫星位姿信息，并保存进缓存队列中；里程计定位融合输人信息处理模块，用于矫正定位位姿数据的偏移量，将矫正里程计位姿数据作为图优化模型的里程计位姿信息，并且使用下一时刻里程计位姿数据和矫正里程计位姿数据更新里程计位姿与矫正里程计位姿的旋转矩阵，将该矩阵作为里程计矫正旋转矩阵；定位融合模块，用于融合卫星位姿信息和里程计位姿信息，包括：从缓存队列中获取历史卫星位姿信息和里程计位姿信息，并时间同步两类位姿信息，将同步后的历史卫星位姿信息和里程计位姿信息使用图优化方法进行优化修正，获取到优化位姿信息，通过优化位姿信息和未优化位姿信息更新未优化位姿信息与优化位姿信息的旋转矩阵，将该矩阵作为图优化融合旋转矩阵；融合定位信息输出模块，用于计算并输出融合定位信息，包括：获取里程计矫正旋转矩阵和图优化融合旋转矩阵，计算二者的乘积获得最终融合定位后的旋转矩阵，将该矩阵作为最终融合结果输出。2.根据权利要求1所述的无人系统多源信息融合定位系统，其特征在于，gnss定位信息模块还包括：获取gngga信息中包含的经纬度、gps质量信息、水平精度因子、天线海拔高度；将这些信息填充人ros的sensor_msgs/navsatfix消息sat中；sat.header.stamp代表时间戳存放此刻时间，sat.status.status代表gps质量信息，sat.latitude代表纬度信息，sat.longitude代表经度信息，sat.altitude代表天线海拔高度、sat.position_covariance[0，4]代表水平精度因子，填充好后将sat信息发送给gps定位融合输入信息处理模块；获取headinga信息(接收速度为1hz)中包含的获取解状态和位置类型、使用卫星数、航向角、基线长，将这些信息填充入ros的geometry_msgs：：quatemionstamped消息qua中，qua.quaternion.x代表解状态和位置类型，x＝1代表解状态为sol_computed且位置类型为narrow_int，其他状态时x＝0，qua.quaternion.y代表使用的卫星数、qua.quaternion.z代表航向角，qua.quaternion.w代表基线长，填充好后将qua信息发送给gps定位融合输入信息处理模块。3.根据权利要求2所述的无人系统多源信息融合定位系统，其特征在于，gnss定位融合输入信息处理模块还包括：该模块负责将地球坐标系的位姿数据转换成emu坐标系的位姿数据，并根据卫星数和基线长等信息判断该位姿信息的质量情况，筛选质量优良的位姿数据作为图优化模型的第一定位融合数据；获取gnss定位信息模块发送的sat信息和qua信息；首先判断sat信息和qua信息的时间
戳是否相同，时间戳相同说明sat信息和qua信息是同一时刻的定位信息；获取同一时间的sat信息与qua信息后，创建一个ros的nav_msgs/odometry信息gnssodom；将sat信息的经纬度使用geographiclib类型的forward方法转换成emu坐标系的位置信息并填人gnssodom.pose.pose.position中；将qua信息中的航向角使用tf库的createquaternionfromyaw方法转换成姿态四元素信息并填入gnssodom.pose.pose.orientation中，此时初始化gnssodom.pose.covariance[0]为0.01，代表gnssodom在融合时位置数据的权重，值越小表示权重越大，初始化gnssodom.twist.covariance[0]为0.05，代表gnssodom在融合时姿态数据的权重，值越小表示权重越大；获取到gnssodom信息后，判断该定位数据的质量是否优良，保证进入融合定位的gnss定位数据是准确的，具体流程如下：(1)判断qua.quaternion.x＜0.8llqua.quaternion.y＜10为true，说明该定位数据质量很差，直接丢弃该gnssodom，并将stable＝0，然后直接退出判断，不再进行其他操作，等待下个sat和qua信息；若为false，则到第2步；stable参数是当gnssodom信息从质量较差恢复到质量好时，要等待gnssodom信息质量良好并保持直到获取下一个gnssodom信息的已等待时间；(2)判断qua.quaternion.y＜15为true，则进人第3步；若为false，则进入第4步；(3)判断0.6＜qua.quaternion.w＜0.65为true，说明此时在卫星数量较少的情况下gnssodom定位精度一般，此时降低gnssodom在融合定位时的权重，将gnssodom.pose.covariance[0]设置为5，gnssodom.twist.covariance[0]设置为0.1，然后进入第4步；若为false，则直接退出，不再进行其他操作，等待下个sat和qua信息；(4)将stable+1，然后进人第5步；(5)判断qua.quaternion.y>17为ture，说明此时gnssodom信息质量很好，将stable+4，以尽快获取gnssodom信息，然后进入第6步；若为false，则直接进入第6步；(6)判断stable＜60为true，则说明gnssodom信息质量良好还未保持一定时间，直接退出判断，不再进行其他操作，等待下个sat和qua信息；若为false，则进入第7步；(7)此刻的gnssodom信息可以作为融合定位图优化模型的卫星位姿信息；当gnssodom信息质量差时，此时无人系统的定位信息只使用里程计数据进行图优化，去除了gnssodom错误定位信息对无人系统定位准确性的干扰。4.根据权利要求3所述的无人系统多源信息融合定位系统，其特征在于，里程计定位融合输人信息处理模块还包括：获取到无人系统底盘发送来的odom信息后，将之标记成curodom，与前一时刻收到的odom信息lastodom进行位置差值运算求得这段时间无人系统的位移数据，计算公式如下：s
i+1
(x，y，z)＝a
i+1
(x，y，z)-a
i
(x，y，z)
a
i
(x，y，z)代表里程计第i时刻里程计位姿数据，由于里程计定位数据偏移，导致计算得到的s
i
(x，y，z)>实际的位移(x，y，z)，因此缩小s
i
(x，y，z)再将位移值加到第i时刻矫正位姿数据中，就能获得第i+1时刻矫正位姿数据，所以当前时刻优化后的位姿数据计算公式如下β
i+1
(x，y，z)＝β
i
(x，y，z)+(s
i+1
(x，y，z)
·
scaler)β
i
(x，y，z)为里程计第i时刻矫正位姿数据，scaler为矫正参数值(scaler＜1)，通过减小计算的位移量，矫正里程计与惯导融合后的定位数据偏移，得到的β
i
(x，y，z)转换成位姿数据odom是更精确的里程计定位数据，将odom作为图优化模型的里程计位姿信息，使用第i+1时刻里程计位姿数据和矫正里程计位姿数据更新里程计位姿与矫正里程计位姿的旋转矩阵，将该矩阵作为里程计矫正旋转矩阵：其中为里程计矫正旋转矩阵，a
i-1
为第i时刻里程计位姿的逆，bi表示第i时刻矫正里程计位姿矩阵。5.根据权利要求4所述的无人系统多源信息融合定位系统，其特征在于，定位融合模块还包括：当缓存队列中的数据量达到20时，获取卫星位姿信息gnssodom和里程计位姿信息odom，比较gnssodom的时间戳gnssodom.header.stamp与odom的时间戳odom.header.stamp是否相等，若相等则将gnssodom与odom送人图优化模型中进行融合定位；图优化模型中，使用如下公式来优化每时刻的里程计位姿信息：t
t
代表t时刻位姿转换到t+1时刻位姿的旋转矩阵，x
t
代表t时刻位姿与t+1时刻位姿差矩阵，h
t
代表t时刻位姿所占权重矩阵。使用如下公式通过每时刻的gps位姿信息来优化里程计位姿信息：使用如下公式通过每时刻的gps位姿信息来优化里程计位姿信息：tg
t
代表t时刻gps的位姿矩阵，xg
t
代表t时刻里程计的位姿矩阵，hg
t
代表t时刻gps位姿所占权重矩阵，p
i
代表损失函数通过图优化计算后，得到融合的位姿信息，使用最后一个时刻的里程计位姿和融合的位姿信息来更新旋转矩阵，计算公式如下：位姿信息来更新旋转矩阵，计算公式如下：代表里程计位姿矩阵转换成融合的位姿矩阵的旋转矩阵，o
α-1
为里程计位姿矩阵的逆，opt
β
表示融合的位姿矩阵；
将作为图优化融合旋转矩阵发送给融合定位信息输出模块。6.根据权利要求5所述的无人系统多源信息融合定位系统，其特征在于，融合定位信息输出模块还包括：该模块获取里程计矫正旋转矩阵与图优化融合旋转矩阵计算统一的转换关系矩阵系矩阵将作为多源融合定位方法的最终输出融合旋转矩阵。

技术总结
本发明提供了一种无人系统多源信息融合定位系统，包括GNSS定位信息模块、GNSS定位融合输入信息处理模块、里程计定位融合输入信息处理模块、定位融合模块、融合定位信息输出模块，用于解决在复杂工作条件下时，由于GNSS信号被干扰、遮挡导致定位信息错误，以及里程计和惯导定位信息随时间增长误差增大的问题，保证在复杂工作条件下，无人系统定位准确性与稳定性。定性。定性。


技术研发人员：秦云川 罗利 徐成 成佳艺
受保护的技术使用者：湖南大学
技术研发日：2023.05.23
技术公布日：2023/9/20