CN113566828A - 一种基于多传感器决策融合的抗冲击扫描匹配方法及系统 - Google Patents
一种基于多传感器决策融合的抗冲击扫描匹配方法及系统 Download PDFInfo
- Publication number
- CN113566828A CN113566828A CN202110780758.1A CN202110780758A CN113566828A CN 113566828 A CN113566828 A CN 113566828A CN 202110780758 A CN202110780758 A CN 202110780758A CN 113566828 A CN113566828 A CN 113566828A
- Authority
- CN
- China
- Prior art keywords
- robot
- frame
- decision
- acceleration
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 70
- 238000000034 method Methods 0.000 title claims abstract description 58
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 19
- 230000001133 acceleration Effects 0.000 claims description 140
- 238000005259 measurement Methods 0.000 claims description 114
- 239000011159 matrix material Substances 0.000 claims description 44
- 230000003068 static effect Effects 0.000 claims description 25
- 230000009466 transformation Effects 0.000 claims description 22
- 238000006073 displacement reaction Methods 0.000 claims description 18
- 238000007781 pre-processing Methods 0.000 claims description 15
- 238000012937 correction Methods 0.000 claims description 14
- 206010034719 Personality change Diseases 0.000 claims description 12
- 238000006243 chemical reaction Methods 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 5
- 241000284466 Antarctothoa delta Species 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 239000000126 substance Substances 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000035939 shock Effects 0.000 claims 3
- 230000008569 process Effects 0.000 description 9
- 230000008447 perception Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000035945 sensitivity Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
Abstract
本发明公开一种基于多传感器决策融合的抗冲击扫描匹配方法及系统，所述算法包括如下步骤：步骤1：通过融合车轮编码器的信息与陀螺仪的信息生成局部第一决策；步骤2：通过融合加速度计的信息与陀螺仪的信息生成局部第二决策；步骤3：根据决策融合规则从局部第一决策和局部第二决策中选取其一作为全局决策；步骤4：将全局决策作为扫描匹配的初始位姿，通过以初始位姿为中心的搜索窗口进行扫描匹配，以获取地图被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配。本发明通过融合决策提供良好的初始位姿估计，使得扫描匹配算法具有抗冲击性，提高扫描匹配算法的鲁棒性和准确性。
Description
技术领域
本发明涉及机器人控制算法领域，具体涉及一种基于多传感器决策融合的抗冲击扫描匹配方法及系统。
背景技术
移动机器人在室内环境中成功的路径规划与导航需要对当前机器人位置的可靠估计。实现这种可靠性的基本思想之一是融合来自多个传感器的信息，这能提高机器人定位的鲁棒性与准确性。目前最流行的移动机器人定位方案往往将激光雷达，车轮编码器以及惯性传感器相结合。然而，在复杂的动态环境下，机器人极易受到横向冲击。当机器人在低摩擦地面上移动时，若机器人轮胎附着力较低，将极易发生侧滑。机器人侧滑一般是指机器人车轮发生横向移动的现象。该现象可被视为机器人的绑架问题，即：机器人在没有先验知识或传感器无感知的情况下被瞬间移动到其他位置，车轮编码器无法感知冲击造成的横向位移，将会为扫描匹配算法提供不准确的先验知识，进而影响扫描匹配结果。
发明内容
为解决上述问题，本发明提供了一种基于多传感器决策融合的抗冲击扫描匹配方法及系统，通过融合决策实现更良好的初始位姿估计，使得扫描匹配算法具有抗冲击性，基于扫描匹配算法提高机器人的定位精准度。本发明的具体技术方案如下：
一种基于多传感器决策融合的抗冲击扫描匹配方法，该算法包括如下步骤：步骤1：通过融合车轮编码器的信息与陀螺仪的信息生成局部第一决策；步骤2：通过融合加速度计的信息与陀螺仪的信息生成局部第二决策；步骤3：根据决策融合规则从局部第一决策和局部第二决策中选取其一作为全局决策；步骤4：将全局决策作为扫描匹配的初始位姿，结合搜索窗口进行扫描匹配，获取占用栅格地图被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配。
与现有技术相比，本发明基于多个传感器的测量结果融合生成两个局部决策，以提高机器人定位的鲁棒性和准确性，通过决策融合规则选取出其中一个决策作为全局决策，为扫描匹配算法提供初始位姿进行扫描匹配，使得扫描匹配算法具有抗冲击性，避免在机器人受到横向冲击时出现扫描匹配不准确的情况。相较于传统的扫描匹配算法，本发明通过多传感器决策融合使得扫描匹配算法具备抗冲击性。
进一步地，所述基于多传感器决策融合的抗冲击扫描匹配方法在执行步骤1前还包括：步骤00：缓存原始数据，判断原始数据的缓存帧数是否已缓存两帧或两帧以上，若是，则进入步骤01；步骤01：对缓存的原始数据进行预处理，获取原始数据的预处理结果；步骤02：判断车轮编码器测量值是否发生变化，若否，则进入步骤03，若是，则确定机器人处于非静止状态，进入步骤05；步骤03：判断滑动窗口内加速度测量值的标准差是否小于预设加速度标准差阈值，若是，则确定机器人处于静止状态，进入步骤04，若否，则确定机器人处于非静止状态，进入步骤05；步骤04：将机器人处于静止状态时先后连续缓存的全部加速度测量值记录为先验知识；步骤05：机器人处于非静止状态，连续缓存的两帧之间加速度测量值的偏置b的更新满足bk+1＝bk，其中，k是原始数据的缓存帧的序号；其中，原始数据包括车轮编码器测量值、陀螺仪测量值、加速度测量值和各个传感器测量值的每一帧所对应的采集时刻；所述对缓存的原始数据进行预处理包括：计算先后连续缓存的两帧车轮编码器测量值差值、计算连续两帧机器人的姿态变化Δθ、计算先后连续缓存的两帧传感器测量值的采集时刻差值Δt、计算滑动窗口内加速度测量值的均值和标准差、将传感器测量值按照采集时刻先后顺序依次存储，以生成传感器测量值序列；将连续两帧机器人的姿态变化Δθ、先后连续缓存的两帧传感器测量值的采集时刻差值Δt、滑动窗口内加速度测量值的均值和标准差以及传感器测量值序列作为所述原始数据的预处理结果；其中，所述传感器测量值序列包括加速度测量值序列、陀螺仪测量值序列和车轮编码器测量值序列；所述滑动窗口内加速度测量值是指基于滑动窗口技术，从加速度测量值序列中提取的最新缓存的预设帧数的加速度测量值。本发明通过对原始数据的预处理，基于原始数据的预处理结果判断机器人的运动状态，根据机器人的运动状态决定是否对传感器的偏置进行校正，以使得传感器测量值值更加精准，减少获取的局部决策或全局决策的误差。
进一步地，当检测到启动后的机器人重新处于静止状态时，将先验知识更新为机器人重新处于静止状态时先后缓存的全部连续时刻帧的加速度测量值。考虑到传感器具有温度敏感等特性，当启动后的机器人重新处于静止状态时，立即更新先验知识，以避免出现先验知识更新不及时导致机器人是否受到横向冲击的判断结果不准确而影响决策融合的情况。
进一步地，所述步骤1具体包括：步骤11：获取第k帧到第k+1帧的机器人的姿态变化Δθ1，获取第k+1帧的机器人姿态θ(k+1)1，θ(k+1)1＝θk1+△θ1；步骤12：定义机器人在机器人行进平面上第k帧的第一坐标变换矩阵Rk1；步骤13：基于机器人车轮直径、第k帧车轮编码器测量值与第k+1帧车轮编码器测量值的差值，获取机器人左轮弧长ΔL和机器人右轮弧长ΔR；步骤14：基于步骤13获取的机器人左轮弧长ΔL和机器人右轮弧长ΔR，获取机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1和机器人从第k帧到第k+1帧在Y轴方向相对位移ΔY1；步骤15：获取第k+1帧的机器人位置
其中，
为第k帧的机器人位置；步骤16：将第k+1帧的机器人位姿信息(X(k+1)1,Y(k+1),θ(k+1)1)作为局部第一决策生成；其中，所述机器人左轮弧长是指机器人左轮上的一点从第k帧到第k+1帧所转过的弧长；所述机器人右轮弧长是指机器人右轮上的一点从第k帧到第k+1帧所转过的弧长；帧数k是大于或等于1的整数。局部第一决策作为全局决策的选项之一，是基于车轮编码器和陀螺仪这两种内部感受型传感器感知机器人的位置和姿态变化进而生成的决策。
进一步地，所述步骤14具体包括：计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的和值的二分之一，将计算结果确认为机器人的中心从第k帧到第k+1帧的移动轨迹长度；当机器人从第k帧到第k+1帧之间转动的圆心角的角度小于或等于预设圆心角阈值时，机器人的中心从第k帧到第k+1帧的移动轨迹长度等于机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1；计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值；计算机器人左轮和机器人右轮之间的直线距离；机器人从第k帧到第k+1帧之间转动的圆心角等于所述机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值与所述机器人左轮和机器人右轮之间的直线距离之比；根据勾股定理，机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1等于所述机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1与所述机器人从第k帧到第k+1帧之间转动的二分之一的圆心角的正切值的乘积；其中，所述机器人的中心是指机器人的左轮与机器人的右轮的连线的中点。
进一步地，所述步骤2具体包括：步骤21：获取第k帧到第k+1帧的机器人的姿态变化Δθ2，获取第k+1帧机器人的姿态θ(k+1)2，θ(k+1)2＝θk2+△θ2；步骤22：定义机器人欧拉角依次为：相对于机器人本体坐标系，绕Z轴旋转为偏航角α，绕Y轴旋转为俯仰角β，绕X轴旋转为横滚角γ，根据机器人欧拉角，定义机器人在所运动的空间中第k帧的第二坐标变换矩阵Rk2；步骤23：根据原始数据中第k帧加速度测量值ak、第k+1帧加速度测量值ak+1、第k帧加速度测量值偏置bk、第k+1帧加速度测量值偏置bk+1、第k帧的第二坐标变换矩阵Rk2和第k+1帧的第二坐标变换矩阵R(k+1)2，获取机器人第k帧时刻和机器人第k+1帧时刻的中点加速度amid；步骤24：获取第k+1帧机器人速度Vk+1，Vk+1＝Vk+a·△t，其中，加速度a为机器人第k帧时刻和机器人第k+1帧时刻的中点加速度amid与重力加速度g的和值；步骤25：根据步骤24获取的第k+1帧的机器人速度传播方程，对第k+1帧的机器人速度进行一次积分，获取第k+1帧机器人位置Pk+1，
其中，Pk+1表示机器人第k+1帧的位置(X(k+1)2，Y(k+1)2，Z(k+1)2)，Pk表示机器人第k帧的位置信息(Xk2，Yk2，Zk2)；步骤26：将第k+1帧的机器人位姿信息(X(k+1)2,Y(k+1)2,θ(k+1)2)作为局部第二决策生成。局部第二决策作为全局决策的选项之一，相较于局部第一决策，局部第二决策是基于外部感受型传感器感知机器人的位置变化，进而生成的决策。经过发明人多次重复试验发现针对机器人受到横向冲击的情况下使用外部感受型传感器辅助内部感受型传感器能够获得更精准的位姿估计。
进一步地，所述步骤23具体包括：计算机器人第k帧加速度测量值ak与机器人第k帧加速度测量值偏置bk的差值并记录为机器人第k帧加速度校正测量值；计算机器人第k+1帧加速度测量值ak+1与机器人第k+1帧加速度测量值偏置bk+1的差值并记录为机器人第k+1帧加速度校正测量值；计算机器人第k帧的第二坐标变换矩阵Rk2与所述机器人第k帧加速度校正测量值的乘积并记录为机器人第k帧加速度变换测量值；计算机器人第k+1帧的第二坐标变换矩阵R(k+1)2与所述机器人第k+1帧加速度校正测量值的乘积并记录为机器人第k+1帧加速度变换测量值；机器人第k帧时刻和机器人第k+1帧时刻的中点加速度等于所述机器人第k帧加速度变换测量值和所述机器人第k+1帧加速度变换测量值的和值的二分之一。
进一步地，所述步骤3具体包括：步骤31：获取传感器测量值序列，然后进入步骤32；步骤32：将所述先验知识中机器人处于静止状态的所有连续时刻帧的加速度测量值作为第一总体，基于预设尺寸的滑动窗口从加速度测量值序列中提取最新缓存的预设帧数的加速度测量值作为第一样本，计算第一样本与第一总体之间的马氏距离，判断第一样本与第一总体之间的马氏距离是否大于预设马氏距离阈值，若是，则进入步骤33，若否，则确定为机器人未受到横向冲击，进入步骤35；步骤33：从陀螺仪测量值序列中提取最新缓存的一帧陀螺仪测量值作为当前时刻陀螺仪测量值，根据当前时刻陀螺仪测量值获取当前时刻机器人横滚角和当前时刻机器人俯仰角，计算当前时刻机器人横滚角与预设机器人横滚角阈值之间的第一欧氏距离，计算当前时刻机器人俯仰角与预设机器人俯仰角阈值之间的第二欧氏距离，判断第一欧氏距离与第二欧氏距离是否都小于预设欧氏距离阈值，若是，则确定为机器人受到横向冲击，进入步骤34，若否，则确定为机器人未受到横向冲击，进入步骤35；步骤34：将局部第二决策选取为全局决策；步骤35：将局部第一决策选取为全局决策；其中，所述滑动窗口的预设尺寸的具体大小由对加速度测量值提取的预设帧数决定。本发明选择马氏距离作为衡量加速度相似程度的距离度量，选择欧氏距离作为衡量欧拉角相似程度的距离度量，通过判断马氏距离和欧氏距离与对应阈值的大小关系，判断机器人的加速度或姿态是否发生剧烈变化，从而确定机器人所处状态对应选取相应局部决策作为全局决策。
进一步地，步骤4所述将全局决策作为扫描匹配的初始位姿，结合搜索窗口进行扫描匹配，获取占用栅格地图中被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配，具体包括：步骤41：将全局决策的第k+1帧机器人的位姿作为扫描匹配的初始位姿；步骤42：将初始位姿中的第k+1帧激光点云的坐标由激光坐标系变换为地图坐标系；步骤43：通过以扫描匹配的初始位姿为中心的搜索窗口，获取占用栅格地图被第k+1帧激光点云占用概率最大的位姿作为最终位姿，将全局决策的机器人第k+1帧的位姿更新为所述最终位姿，完成全局决策的机器人第k+1帧位姿的扫描匹配。与传统的扫描匹配算法相比较，本发明基于决策融合规则，在机器人受到横向冲击侧滑的情况下，选用外部感受型传感器辅助内部感受型传感器的方式，使得本发明的扫描匹配算法更具有抗冲击性，扫描匹配效果更佳，有效提高机器人的定位精度。
进一步地，步骤43中使用的所述搜索窗口的尺寸是由如下步骤决定的：计算机器人位姿协方差矩阵；根据所述机器人位姿协方差矩阵，获取机器人位置误差椭圆；判断所述机器人位置误差椭圆的主轴长度是否大于预设搜索窗口尺寸初值；若机器人位置误差椭圆的主轴长度大于预设搜索窗口初值，则使用机器人位置误差椭圆的主轴长度作为搜索窗口的尺寸；若机器人位置误差椭圆的主轴长度小于或等于预设搜索窗口尺寸初值，则使用预设搜索窗口尺寸初值作为搜索窗口的尺寸。本发明中搜索窗口的尺寸根据机器人位姿协方差矩阵实现自适应调整，以使得搜索窗口的尺寸自适应调节为最合适机器人当前情况的大小。
进一步地，当一次全局决策的扫描匹配完成后，将机器人位姿协方差矩阵重置为预设机器人位姿协方差矩阵初值，将搜索窗口的尺寸重置为预设搜索窗口尺寸初值。本发明在每一次全局决策扫描匹配后都对搜索窗口的尺寸进行重置为初值的操作，以确保不同帧全局决策的扫描匹配的结果不受搜索窗口尺寸变化的干扰。
本发明还公开了一种基于多传感器决策融合的抗冲击扫描匹配系统，所述基于多传感器决策融合的抗冲击扫描匹配系统执行如前所述的基于多传感器决策融合的抗冲击扫描匹配方法，所述基于多传感器决策融合的抗冲击扫描匹配系统包括：车轮编码器，用于检测机器人车轮的位移信息；三轴陀螺仪，用于检测机器人角速度信息；三轴加速度计，用于检测机器人加速度信息；激光雷达，用于获取激光点云数据；分布式检测系统，包括决策器一和决策器二，用于基于多个传感器的测量值生、成一条以上的局部决策并传输至决策融合系统；决策融合系统，用于接收分布式检测系统传输的一条以上的局部决策，结合决策融合规则生成全局决策；扫描匹配单元，用于基于决策融合系统生成的全局决策在占用栅格地图中通过搜索窗口扫描匹配以获取占用栅格地图被激光点云占用概率最大的扫描位姿。本发明公开的基于多传感器决策融合的抗冲击扫描匹配系统，基于外部感受型传感器和内部感受型传感器的测量值分布式检测系统生成多条局部决策，决策融合系统基于决策融合规则和局部决策生成全局决策，实现扫描匹配系统的抗冲击性，提高扫描匹配系统的准确性。
附图说明
图1为本发明第一实施例所述基于多传感器决策融合的抗冲击扫描匹配方法的流程示意图。
图2为本发明第二实施例所述基于多传感器决策融合的抗冲击扫描匹配方法的详细流程示意图。
图3为本发明第二实施例所述基于车轮编码器的运动学模型的结构示意图。
图4为本发明一种实施例所述基于多传感器决策融合的抗冲击扫描匹配系统的结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清晰，以下将结合附图及实施例，对本发明进行描述和说明。应当理解，下面所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。此外，还可以理解的是，对本领域的普通技术人员而言，在本发明揭露的技术内容上进行一些设计，制造或者生产等变更只是常规的技术手段，不应当理解为本申请公开的内容不充分。
除非另作定义，本发明所涉及的技术术语或科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等词语并不表示数量限制，可以表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形，意图在于覆盖不排他的包含，如：包含了一系列步骤或模块的过程、方法、系统产品或者设备没有限定于已列出的步骤或单元，而是可以还包括没有列出的步骤或模块，或者还可以包括对于这些过程、方法、产品或设备固有的其他步骤或单元。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是用于区别类似的对应，不代表针对对象的特定排序。
本发明的第一实施例中提供一种基于多传感器决策融合的抗冲击扫描匹配方法，图1是所述基于多传感器决策融合的抗冲击扫描匹配方法的流程示意图，如图1所示，所述基于多传感器决策融合的抗冲击扫描匹配方法包括如下步骤：
步骤1：通过融合车轮编码器的信息与陀螺仪的信息生成局部第一决策；具体地，所述车轮编码器的信息反映机器人车轮的位置变化，所述陀螺仪的信息反映机器人姿态变化；所述局部第一决策是指通过融合车轮编码器的信息和陀螺仪的信息所获取的位姿对应生成的决策。本步骤通过融合内部感受型传感器的测量值以获取决策，由于车轮编码器存在无法良好检测机器人车轮的横向位移的问题，使得局部第一决策在机器人受到横向冲击时存在局限性，降低了扫描匹配的准确性。
步骤2：通过融合加速度计的信息与陀螺仪的信息生成局部第二决策；具体地，所述加速度计的信息反映机器人的加速度变化。本步骤通过融合外部感受型传感器和陀螺仪的测量值以获取局部第二决策，经过多次试验发现加速度计在机器人受到横向冲击时加速度计的测量值发生剧烈变化，通过检测加速度计的测量值能够较好的检测机器人车轮发生横向冲击的问题，解决了局部第一决策存在局限性的问题。
步骤3：根据决策融合规则从局部第一决策和局部第二决策中选取其一作为全局决策；具体地，决策融合规则是依据机器人所处状态从局部第一决策和局部第二决策中选取一个局部决策作为全局决策的规则；优选地，所述决策融合规则可以是但不限于通过判断机器人是否受到横向冲击进而根据判断结果选择决策，在这种决策融合规则下生成的全局决策使得扫描匹配算法具备抗冲击性，降低因机器人受到横向冲击对扫描匹配结果产生的影响。
步骤4：将全局决策作为扫描匹配的初始位姿，结合搜索窗口进行扫描匹配，获取占用栅格地图被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配。其中，所述搜索窗口以所述扫描匹配的初始位姿为中心。
优选地，步骤3中所述选取全局决策的步骤具体包括：根据原始数据的预处理结果，获取传感器测量值序列；从传感器测量值序列提取一个以上的预设数帧的特征子序列；对所述提取的一个以上的预设数帧特征子序列按照特征进行分类；根据分类结果结合决策融合规则，选取局部第一决策或局部第二决策作为全局决策；其中，所述决策融合规则可以是但不限于根据判断机器人是否受到横向冲击进而选取一种局部决策作为全局决策的规则；所述特征子序列的帧数可以根据实际需求调整配置。
本发明的第二实施例中提供了另一种基于多传感器决策融合的抗冲击扫描匹配方法，图2为第二实施例所述基于多传感器决策融合的抗冲击扫描匹配方法的具体步骤流程图，如图2所示，所述基于多传感器决策融合的抗冲击扫描匹配方法包括如下步骤：
步骤101：缓存原始数据，判断原始数据是否已缓存两帧或两帧以上，若是，则进入步骤102；具体地，所述原始数据包括车轮编码器测量值、陀螺仪测量值、加速度测量值和时间；所述时间是指各个传感器测量值的每一帧测量值所对应的采集时刻。
步骤102：对已缓存的原始数据进行预处理，然后进入步骤103；具体地，所述原始数据的预处理包括：计算先后连续缓存的两帧车轮编码器测量值差值、计算先后连续缓存的两帧机器人的姿态变化Δθ、计算先后连续缓存的两帧传感器测量值的采集时刻差值Δt、计算滑动窗口内加速度测量值的均值和标准差、将传感器测量值按照采集时刻先后顺序依次存储，以生成传感器测量值序列；所述传感器测量值序列包括加速度测量值序列、陀螺仪测量值序列和车轮编码器测量值序列。将先后连续缓存的两帧机器人的姿态变化Δθ、先后连续缓存的两帧传感器测量值的采集时刻差值Δt、滑动窗口内加速度测量值的均值和标准差以及传感器测量值序列作为原始数据的预处理结果。
步骤103：判断车轮编码器测量值是否发生变化，若否，则进入步骤104，若是，则进入步骤106；具体地，所述判断车轮编码器测量值是否发生变化是根据原始数据的预处理结果中先后连续缓存的两帧车轮编码器测量值的差值判断的，若连续两帧车轮编码器测量值的差值不为零，则代表这两帧之间车轮编码器测量值发生了变化，若连续两帧的车轮编码器测量值差值为零，则代表这两帧之间车轮编码器测量值未发生变化。
步骤104：判断滑动窗口内加速度测量值的标准差是否小于预设加速度标准差阈值，若是，则进入步骤105，若否，则进入步骤106；其中，所述滑动窗口是指基于滑动窗口技术用于提取最新缓存的连续预设帧数的传感器测量值的窗口；所述滑动窗口内加速度测量值是指基于滑动窗口技术，从加速度测量值序列中提取的最新缓存的预设帧数的加速度测量值；所述预设加速度标准差阈值是发明人经过多次试验后确定的用于限定加速度测量值的合理变化幅度的值；所述合理变化幅度是指机器人处于静止状态时通过计算加速度测量值的加速度标准差阈值所反映的变化幅度；具体地，本步骤是通过比较原始数据预处理结果中滑动窗口内加速度测量值的标准差是否小于预设加速度标准差阈值，以判断滑动窗口内加速度测量值的变化幅度是否在合理变化幅度内，若滑动窗口内加速度测量值的标准差小于预设加速度标准差阈值，则表示滑动窗口内的连续数帧的加速度测量值的变化幅度在合理变化幅度内，反之，若滑动窗口内加速度测量值的标准差大于或等于预设加速度标准差阈值，则表示滑动窗口内的连续数帧的加速度测量值的变化幅度不属于合理变化幅度内。
步骤105：确定机器人处于静止状态，将加速度测量值的偏置更新为滑动窗口内加速度测量值的均值，将机器人处于静止状态时先后连续缓存的全部连续时刻帧的加速度测量值记录为先验知识；具体地，由于加速度计具有温度敏感等特性，测量累计误差较大，因此通过更新加速度测量值的偏置，以实现对加速度测量值的校正，减少误差影响；出于算法的空间复杂性以及传感器个体之间的差异性的考虑，仅存储机器人处于静止状态时的加速度测量值作为先验知识。
优选地，当检测到启动后的机器人重新处于静止状态时，将先验知识更新为机器人重新处于静止状态时先后连续缓存的全部连续时刻帧的加速度测量值。
步骤106：确定机器人处于非静止状态，加速度测量值的偏置b的更新满足bk+1＝bk，其中，k是原始数据的缓存帧的序号；可以理解地，机器人处于非静止状态时，加速度测量值的偏置b的更新后的偏置值bk+1与更新前的偏置值bk相同。
步骤201：获取第k帧到第k+1帧的机器人的姿态变化Δθ1，获取第k+1帧的机器人姿态θ(k+1)1，θ(k+1)1＝θk1+△θ1；具体地，第k帧到第k+1帧的机器人的姿态变化Δθ1是通过对原始数据中的第k帧到第k+1帧的陀螺仪测量值进行积分所得到的预处理结果。
步骤202：根据机器人第k帧姿态信息θk1，定义机器人在机器人行进平面上第k帧的第一坐标变换矩阵Rk1；具体地，所述第k帧的第一坐标变换矩阵Rk1的表达式为
步骤203：基于机器人车轮直径、第k帧与第k+1帧之间的车轮编码器测量值差值，获取机器人左轮弧长ΔL和机器人右轮弧长ΔR；具体地，图3是基于车轮编码器的运动学模型结构示意图，如图3所示，所述机器人左轮弧长ΔL是指机器人左轮上一点从第k帧到第k+1帧所转过的弧长；所述机器人右轮弧长ΔR是指机器人右轮上一点从第k帧到第k+1帧所转过的弧长；具体地，本步骤中基于机器人车轮直径和机器人车轮编码器测量值计算机器人左右轮转动弧长，属于机器人定位技术中的航迹推算领域，具体获取机器人左轮弧长ΔL和机器人右轮弧长ΔR的推算方法可以是但不限于基于车轮编码器获取机器人左右轮转速，结合机器人车轮直径，获取机器人行进速度，基于机器人行进速度和机器人行进时间进而获取机器人左轮弧长ΔL和右轮弧长ΔR。
步骤204：基于机器人左轮弧长ΔL和机器人右轮弧长ΔR，获取机器人从第k帧到第k+1帧的X轴坐标相对位移ΔX1和机器人从第k帧到第k+1帧的Y轴相对位移ΔY1；具体地，如图3所示，ΔCenter是指机器人的中心(定义为机器人左轮和机器人右轮连线的中点)在第k帧到第k+1帧之间的移动轨迹长度。
所述步骤204具体包括：计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的和值的二分之一，将计算结果确认为机器人的中心从第k帧到第k+1帧的移动轨迹长度；当机器人从第k帧到第k+1帧之间转动的圆心角的角度小于或等于预设圆心角阈值时，机器人的中心从第k帧到第k+1帧的移动轨迹长度等于机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1；计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值；计算机器人左轮和机器人右轮之间的直线距离；机器人从第k帧到第k+1帧之间转动的圆心角等于所述机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值与所述机器人左轮和机器人右轮之间的直线距离之比；根据勾股定理，机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1等于所述机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1与所述机器人从第k帧到第k+1帧之间转动的二分之一的圆心角的正切值的乘积；其中，所述机器人的中心是指机器人的左轮与机器人的右轮的连线的中点；所述预设圆心角阈值是发明人根据实际圆心角的近似条件设定的角度，当机器人转动的圆心角小于或等于预设圆心角阈值时，可以将机器人转动的圆心角视为极小值，从而确定机器人转动的圆心角等于机器人在Y轴方向的相对位移ΔY1。如图3所示，圆心是在机器人左轮中心与机器人右轮中心的连线的延长线上距离机器人的中心预设距离的位置，圆心角是指机器人的中心上一位置与圆心的连线与机器人的中心当前位置与圆心的连线所组成的夹角。
步骤205：根据第一坐标变换矩阵Rk1、所述X轴相对位移ΔX1和Y轴相对位移ΔY1，获取第k+1帧的机器人位置
其中，
为第k帧的机器人位置。
步骤206：将第k+1帧的机器人位姿信息(X(k+1)1,Y(k+1),θ(k+1)1)作为局部第一决策生成；其中，k是大于或等于1的整数。
优选地，步骤201至步骤206为基于内部感受型传感器生成局部第一决策的具体流程。
步骤301：获取第k帧到第k+1帧的机器人的姿态变化Δθ2，获取第k+1帧机器人的姿态θ(k+1)2，θ(k+1)2＝θk2+△θ2；其中，θk2为第k帧机器人的姿态。
步骤302：定义机器人欧拉角依次为：相对于机器人本体坐标系，绕Z轴旋转为偏航角α，绕Y轴旋转为俯仰角β，绕X轴旋转为横滚角γ，根据机器人欧拉角，定义机器人在机器人所运动的室内空间中第k帧的第二坐标变换矩阵Rk2；具体地，所述第k帧的第二坐标变换矩阵Rk2的表达式为
步骤303：根据原始数据中第k帧加速度测量值ak、第k+1帧加速度测量值ak+1、第k帧加速度测量值偏置bk、第k+1帧加速度测量值偏置bk+1、第k帧的第二坐标变换矩阵Rk2和第k+1帧的第二坐标变换矩阵R(k+1)2，计算机器人第k帧时刻和机器人第k+1帧时刻的中点加速度amid；具体地，所述步骤303具体包括：计算机器人第k帧加速度测量值ak与机器人第k帧加速度测量值偏置bk的差值并记录为机器人第k帧加速度校正测量值；计算机器人第k+1帧加速度测量值ak+1与机器人第k+1帧加速度测量值偏置bk+1的差值并记录为机器人第k+1帧加速度校正测量值；计算机器人第k帧的第二坐标变换矩阵Rk2与所述机器人第k帧加速度校正测量值的乘积并记录为机器人第k帧加速度变换测量值；计算机器人第k+1帧的第二坐标变换矩阵R(k+1)2与所述机器人第k+1帧加速度校正测量值的乘积并记录为机器人第k+1帧加速度变换测量值；机器人第k帧时刻和机器人第k+1帧时刻的中点加速度等于所述机器人第k帧加速度变换测量值和所述机器人第k+1帧加速度变换测量值的和值的二分之一。
步骤304：根据运动学速度方程获取第k+1帧机器人速度Vk+1，Vk+1＝Vk+a·△t，其中，Vk为第k帧机器人速度，加速度a为机器人第k帧时刻和机器人第k+1帧时刻的中点加速度amid与重力加速度g的和值，Δt为第k帧到第k+1帧之间传感器测量值的采集时刻差值。
步骤305：根据步骤24获取的第k+1帧机器人速度，对第k+1帧机器人速度进行一次积分，获取第k+1帧机器人位置Pk+1，
其中，Pk+1表示机器人第k+1帧的位置(X(k+1)2，Y(k+1)2，Z(k+1)2)，Pk表示机器人第k帧的位置信息(Xk2，Yk2，Zk2)；具体地，根据微积分知识可知，对机器人速度进行一次积分可以获得机器人位置，本技术方案中对步骤304获得的第k+1帧机器人速度进行一次积分，从而获得第k+1帧机器人位置。
步骤306：将第k+1帧的机器人位姿信息(X(k+1)2,Y(k+1)2,θ(k+1)2)作为局部第二决策生成。
优选地，步骤301至步骤306为基于外部感受型传感器辅助内部感受型传感器生成局部第二决策的具体流程。所述生成局部第一决策的具体流程与所述生成局部第二决策的具体流程可以是但不限于两个流程同时进行，两个流程先后进行；其中两个流程的先后进行顺序可以是先执行生成局部第一决策的具体流程，也可以是先执行局部第二决策的具体流程。
步骤401：获取传感器测量值序列；其中，所述传感器测量值序列包括：车轮编码器测量值序列、加速度测量值序列和陀螺仪测量值序列。
步骤402：将所述先验知识中机器人处于静止状态的全部连续时刻帧的加速度测量值作为第一总体，基于预设尺寸的滑动窗口从传感器测量值序列中提取最新缓存的预设帧数的加速度测量值作为第一样本，计算第一样本与第一总体之间的马氏距离，判断第一样本与第一总体之间的马氏距离是否大于预设马氏距离阈值，若是，则进入步骤403，若否，则进入步骤405；其中，所述滑动窗口的尺寸可视为机器人状态的持续时间，作为可调参数直接影响决策融合的结果；所述马氏距离是由印度统计学加马哈拉诺比斯提出的，用于表示数据的协方差距离，它考虑到各种特性之间的联系。在本步骤中使用马氏距离作为衡量加速度时间序列相似程度的距离度量，可用于判断机器人的加速度是否剧烈变化；发明人通过多次仿真结果确定了滑动窗口的预设尺寸,确定了机器人的处于静止状态时加速度合理变化的最大幅度下对应的马氏距离作为预设马氏距离阈值；所述滑动窗口的预设尺寸的具体大小由发明人对加速度测量值所需提取的预设帧数决定。
步骤403：从陀螺仪测量值序列中提取最新缓存的一帧陀螺仪测量值作为当前时刻陀螺仪测量值，根据当前时刻陀螺仪测量值获取当前时刻机器人横滚角和当前时刻机器人俯仰角，计算当前时刻机器人横滚角与预设机器人横滚角阈值之间的第一欧氏距离，计算当前时刻机器人俯仰角与预设机器人俯仰角阈值之间的第二欧氏距离，判断第一欧氏距离与第二欧氏距离是否都小于预设欧氏距离阈值，若是，则进入步骤404，若否，则进入步骤405；其中，所述欧氏距离是指欧几里得度量，用于表示两个点之间的距离；所述当前时刻陀螺仪测量值包括当前时刻机器人横滚角、当前时刻机器人俯仰角和当前时刻机器人偏航角。在本步骤中使用欧氏距离作为衡量欧拉角相似程度的距离度量，可用于判断机器人的姿态是否发生剧烈变化；所述预设机器人横滚角阈值和所述预设机器人俯仰角阈值是发明人通过多次仿真结果确定的用于判断机器人的姿态是否发生明显变化的两个角度阈值；所述预设欧氏距离阈值是发明人通过多次仿真结果确定的用于判断判断机器人的姿态是否发生剧烈变化的距离阈值。
步骤404：确定为机器人受到横向冲击，将局部第二决策选取为全局决策，然后进入步骤501；
步骤405：确定为机器人未受到横向冲击，将局部第一决策选取为全局决策，然后进入步骤501；
具体地，步骤401至步骤405为机器人选取全局决策的具体流程。本实施例通过对局部决策基于决策融合规则进行优化，根据实际机器人状态确定全局决策，为扫描匹配算法提供良好的初始位姿，以提高机器人的定位精准度。
步骤501：将全局决策的第k+1帧机器人的位姿信息作为扫描匹配的初始位姿，然后进入步骤502；具体地，根据全局决策选取的局部决策信息，所述第k+1帧机器人的位姿信息可以是(X(k+1)1,Y(k+1),θ(k+1)1)，也可以是(X(k+1)2,Y(k+1)2,θ(k+1)2)。
步骤502：根据扫描匹配的初始位姿，将初始位姿中的第k+1帧激光点云的坐标由激光坐标系变换为地图坐标系；本步骤对激光点云的坐标进行坐标系变换，以便于后续将激光点云坐标在占用栅格地图中进行扫描匹配。
步骤503：在占用栅格地图中，通过以扫描匹配的初始位姿为中心的搜索窗口获取占用栅格地图被第k+1帧激光点云占用概率最大的位姿作为最终位姿，将全局决策的机器人第k+1帧的位姿更新为所述最终位姿，完成全局决策的机器人第k+1帧位姿的扫描匹配；其中，所述占用栅格地图中每一个栅格点都存在一一对应的被占用概率；本步骤的扫描匹配主要通过将激光点云投影到占用栅格地图上，通过以初始位姿为中心的搜索窗口遍历所有可能的位姿，寻找激光点云与地图重合程度最高的位姿(即被占用概率最大的位姿)，作为初始位姿的最终位姿，以达到扫描匹配后提高机器人定位精准度的目的，将全局决策的机器人第k+1帧的位姿更新为所述最终位姿，提高机器人第k+1位姿的精准度和可信度，降低机器人定位鲁棒性。
优选地，所述搜索窗口的尺寸是可自适应调节的，本发明的第三实施例在前述实施例的基础上提供一种基于多传感器决策融合的抗冲击扫描匹配方法，该算法中扫描匹配使用的搜索窗口的尺寸由如下步骤自适应调节：
步骤S1：计算机器人位姿协方差矩阵，然后进入步骤S2；其中，所述机器人位姿协方差矩阵的计算方法可以是但不限于基于状态方程关于状态向量的雅可比矩阵、状态方程关于控制向量的雅可比矩阵和控制向量噪声协方差矩阵进行计算。
步骤S2：根据所述机器人位姿协方差矩阵获取机器人位置误差椭圆，然后进入步骤S3；具体地，所述获取机器人位置误差椭圆的方法可以是但不限于基于机器人位姿协方差变换以获取特征向量和特征值，基于特征向量确定位姿最大传播方向，基于特征值确定传播广度，其中，机器人位置误差椭圆的主轴长度由特征值决定；具体基于机器人位姿协方差矩阵获取机器人位置误差椭圆的方法还可以是通过C++或Matlab等编程语言软件基于机器人位姿协方差矩阵绘制生成机器人位置误差椭圆。
步骤S3：判断所述机器人位置误差椭圆的主轴长度是否大于预设搜索窗口尺寸初值，若是，则进入步骤S41，若否，则进入步骤S42；具体地，由于机器人位姿协方差矩阵直接地影响了搜索窗口的尺寸，间接地影响了扫描匹配算法的效果，因此发明人通过改变机器人协方差矩阵的初值，经过多次计算仿真实验的扫描匹配结果，确定最合适的机器人位姿协方差矩阵的初值，进而确定了最合适的预设搜索窗口尺寸初值作为预设搜索窗口尺寸初值。
步骤S41：若机器人位置误差椭圆的主轴长度大于预设搜索窗口初值，则使用机器人位置误差椭圆的主轴长度作为搜索窗口的尺寸；
步骤S42：若机器人位置误差椭圆的主轴长度小于或等于预设搜索窗口尺寸初值，则使用预设搜索窗口尺寸初值作为搜索窗口的尺寸。
本实施例中根据机器人位姿协方差矩阵进行搜索窗口的自适应调节，使得搜索窗口的尺寸大小能够根据实际机器人位姿变化，提高搜索窗口尺寸调节灵活性。
优选地，当一次全局决策的扫描匹配完成后，将机器人位姿协方差矩阵重置为预设机器人位姿协方差矩阵初值，将搜索窗口的尺寸重置为预设搜索窗口尺寸初值。
本发明的第四实施例提供了一种基于多传感器决策融合的抗冲击扫描匹配系统，该系统执行如前述实施例任意一种所述的基于多传感器决策融合的抗冲击扫描匹配方法，如图4所示，该系统包括：车轮编码器，用于检测机器人车轮的转速、位移信息；陀螺仪，用于采集机器人角速度信息；加速度计，用于采集机器人加速度信息；激光雷达，用于采集激光点云数据；数据处理单元，用于对车轮编码器、陀螺仪、加速度计等传感器测量值进行数据处理；分布式检测单元，包括决策器一和决策器二，用于基于数据处理单元的数据处理结果生成一条以上的局部决策并传输至决策融合系统；决策融合单元，用于接收分布式检测单元传输的一条以上的局部决策，结合决策融合规则生成全局决策；扫描匹配单元，用于基于决策融合系统生成的全局决策在占用栅格地图中通过搜索窗口扫描匹配以获取占用栅格地图被激光点云占用概率最大的位姿。
在本发明所提供的实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。例如，以上所描述的系统实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。以上所描述的方法仅仅是示意性的，例如：所述步骤的划分，实际实现时可以有另外的划分方式，例如步骤执行的先后顺序可以根据实际调整，或一些步骤可以忽略或不执行。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。上述提到的存储介质可以是只读存储器，磁盘或光盘等。
以上各实施例仅表达了本发明的几种实施例，其描述较为具体和详细，但并不能因此而理解为发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。
Claims (12)
1.一种基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，该算法包括如下步骤：
步骤1：通过融合车轮编码器的信息与陀螺仪的信息生成局部第一决策；
步骤2：通过融合加速度计的信息与陀螺仪的信息生成局部第二决策；
步骤3：从局部第一决策和局部第二决策中选取其一作为全局决策；
步骤4：将全局决策作为扫描匹配的初始位姿，结合搜索窗口进行扫描匹配，获取占用栅格地图被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配。
2.根据权利要求1所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述基于多传感器决策融合的抗冲击扫描匹配方法在执行步骤1前还包括：
步骤00：缓存原始数据，判断原始数据的缓存帧数是否已缓存两帧或两帧以上，若是，则进入步骤01；
步骤01：对缓存的原始数据进行预处理，获取原始数据的预处理结果；
步骤02：判断车轮编码器测量值是否发生变化，若否，则进入步骤03，若是，则确定机器人处于非静止状态，进入步骤05；
步骤03：判断滑动窗口内加速度测量值的标准差是否小于预设加速度标准差阈值，若是，则确定机器人处于静止状态，进入步骤04，若否，则确定机器人处于非静止状态，进入步骤05；
步骤04：将加速度测量值的偏置更新为滑动窗口内加速度测量值的均值，将机器人处于静止状态时先后连续缓存的全部加速度测量值记录为先验知识；
步骤05：机器人处于非静止状态，连续缓存的两帧之间加速度测量值的偏置b的更新满足bk+1＝bk，其中，k是原始数据的缓存帧的序号；
其中，原始数据包括车轮编码器测量值、陀螺仪测量值、加速度测量值和各个传感器测量值的每一帧所对应的采集时刻；所述对缓存的原始数据进行预处理包括：计算先后连续缓存的两帧车轮编码器测量值差值、计算连续两帧机器人的姿态变化Δθ、计算先后连续缓存的两帧传感器测量值的采集时刻差值Δt、计算滑动窗口内加速度测量值的均值和标准差、将传感器测量值按照采集时刻先后顺序依次存储，以生成传感器测量值序列；将连续两帧机器人的姿态变化Δθ、先后连续缓存的两帧传感器测量值的采集时刻差值Δt、滑动窗口内加速度测量值的均值和标准差、以及传感器测量值序列作为所述原始数据的预处理结果；其中，所述传感器测量值序列包括加速度测量值序列、陀螺仪测量值序列和车轮编码器测量值序列；所述滑动窗口内加速度测量值是指基于滑动窗口技术，从加速度测量值序列中提取的最新缓存的预设帧数的加速度测量值。
3.根据权利要求2所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，当检测到启动后的机器人重新处于静止状态时，将先验知识更新为机器人重新处于静止状态时先后缓存的全部连续时刻帧的加速度测量值。
4.根据权利要求2所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述步骤1具体包括：
步骤11：获取第k帧到第k+1帧的机器人的姿态变化Δθ1，获取第k+1帧的机器人姿态θ(k+1)1，θ(k+1)1＝θk1+△θ1；
步骤12：定义机器人在机器人行进平面上第k帧的第一坐标变换矩阵Rk1；
步骤13：基于机器人车轮直径、第k帧车轮编码器测量值与第k+1帧车轮编码器测量值的差值，获取机器人左轮弧长ΔL和机器人右轮弧长ΔR；
步骤14：基于步骤13获取的机器人左轮弧长ΔL和机器人右轮弧长ΔR，获取机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1和机器人从第k帧到第k+1帧在Y轴方向相对位移ΔY1；
步骤15：获取第k+1帧的机器人位置
其中，
为第k帧的机器人位置；
步骤16：将第k+1帧的机器人位姿信息(X(k+1)1,Y(k+1)1,θ(k+1)1)作为局部第一决策生成；
其中，所述机器人左轮弧长是指机器人左轮上的一点从第k帧到第k+1帧所转过的弧长；所述机器人右轮弧长是指机器人右轮上的一点从第k帧到第k+1帧所转过的弧长；帧数k是大于或等于1的整数。
5.根据权利要求4所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述步骤14具体包括：
计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的和值的二分之一，将计算结果确认为机器人的中心从第k帧到第k+1帧的移动轨迹长度；
当机器人从第k帧到第k+1帧之间转动的圆心角的角度小于或等于预设圆心角阈值时，机器人的中心从第k帧到第k+1帧的移动轨迹长度等于机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1；
计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值；
计算机器人左轮和机器人右轮之间的直线距离；
机器人从第k帧到第k+1帧之间转动的圆心角等于所述机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值与所述机器人左轮和机器人右轮之间的直线距离之比；
根据勾股定理，机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1等于所述机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1与所述机器人从第k帧到第k+1帧之间转动的二分之一的圆心角的正切值的乘积；
其中，所述机器人的中心是指机器人的左轮与机器人的右轮的连线的中点。
6.根据权利要求2所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述步骤2具体包括：
步骤21：获取第k帧到第k+1帧的机器人的姿态变化Δθ2，获取第k+1帧机器人的姿态θ(k+1)2，θ(k+1)2＝θk2+△θ2；
步骤22：定义机器人欧拉角依次为：相对于机器人本体坐标系，绕Z轴旋转为偏航角α，绕Y轴旋转为俯仰角β，绕X轴旋转为横滚角γ，根据机器人欧拉角，定义机器人在所运动的空间中第k帧的第二坐标变换矩阵Rk2；
步骤23：根据原始数据中第k帧加速度测量值ak、第k+1帧加速度测量值ak+1、第k帧加速度测量值偏置bk、第k+1帧加速度测量值偏置bk+1、第k帧的第二坐标变换矩阵Rk2和第k+1帧的第二坐标变换矩阵R(k+1)2，获取机器人第k帧时刻和机器人第k+1帧时刻的中点加速度amid；
步骤24：获取第k+1帧机器人速度Vk+1，Vk+1＝Vk+a·△t，其中，加速度a为机器人第k帧时刻和机器人第k+1帧时刻的中点加速度amid与重力加速度g的和值；
步骤25：根据步骤24获取的第k+1帧的机器人速度传播方程，对第k+1帧的机器人速度进行一次积分，获取第k+1帧机器人位置Pk+1，
其中，Pk+1表示机器人第k+1帧的位置(X(k+1)2，Y(k+1)2，Z(k+1)2)，Pk表示机器人第k帧的位置信息(Xk2，Yk2，Zk2)；
步骤26：将第k+1帧的机器人位姿信息(X(k+1)2,Y(k+1)2,θ(k+1)2)作为局部第二决策生成。
7.根据权利要求6所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述步骤23具体包括：
计算机器人第k帧加速度测量值ak与机器人第k帧加速度测量值偏置bk的差值并记录为机器人第k帧加速度校正测量值；
计算机器人第k+1帧加速度测量值ak+1与机器人第k+1帧加速度测量值偏置bk+1的差值并记录为机器人第k+1帧加速度校正测量值；
计算机器人第k帧的第二坐标变换矩阵Rk2与所述机器人第k帧加速度校正测量值的乘积并记录为机器人第k帧加速度变换测量值；
计算机器人第k+1帧的第二坐标变换矩阵R(k+1)2与所述机器人第k+1帧加速度校正测量值的乘积并记录为机器人第k+1帧加速度变换测量值；
机器人第k帧时刻和机器人第k+1帧时刻的中点加速度等于所述机器人第k帧加速度变换测量值和所述机器人第k+1帧加速度变换测量值的和值的二分之一。
8.根据权利要求2所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述步骤3具体包括：
步骤31：获取传感器测量值序列，然后进入步骤32；
步骤32：将所述先验知识中机器人处于静止状态的所有连续时刻帧的加速度测量值作为第一总体，基于预设尺寸的滑动窗口从加速度测量值序列中提取最新缓存的预设帧数的加速度测量值作为第一样本，计算第一样本与第一总体之间的马氏距离，判断第一样本与第一总体之间的马氏距离是否大于预设马氏距离阈值，若是，则进入步骤33，若否，则确定为机器人未受到横向冲击，进入步骤35；
步骤33：从陀螺仪测量值序列中提取最新缓存的一帧陀螺仪测量值作为当前时刻陀螺仪测量值，根据当前时刻陀螺仪测量值获取当前时刻机器人横滚角和当前时刻机器人俯仰角，计算当前时刻机器人横滚角与预设机器人横滚角阈值之间的第一欧氏距离，计算当前时刻机器人俯仰角与预设机器人俯仰角阈值之间的第二欧氏距离，判断第一欧氏距离与第二欧氏距离是否都小于预设欧氏距离阈值，若是，则确定为机器人受到横向冲击，进入步骤34，若否，则确定为机器人未受到横向冲击，进入步骤35；
步骤34：将局部第二决策选取为全局决策；
步骤35：将局部第一决策选取为全局决策；
其中，所述滑动窗口的预设尺寸的大小由对加速度测量值提取的预设帧数决定。
9.根据权利要求2所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，步骤4所述将全局决策作为扫描匹配的初始位姿，结合搜索窗口进行扫描匹配，获取占用栅格地图中被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配，具体包括：
步骤41：将全局决策的第k+1帧机器人的位姿作为扫描匹配的初始位姿；
步骤42：将初始位姿中的第k+1帧激光点云的坐标由激光坐标系变换为地图坐标系；
步骤43：通过以扫描匹配的初始位姿为中心的搜索窗口，获取占用栅格地图被第k+1帧激光点云占用概率最大的位姿作为最终位姿，将全局决策的机器人第k+1帧的位姿更新为所述最终位姿，完成全局决策的机器人第k+1帧位姿的扫描匹配。
10.根据权利要求9所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，步骤43中使用的所述搜索窗口的尺寸是由如下步骤决定的：
计算机器人位姿协方差矩阵；
根据所述机器人位姿协方差矩阵，获取机器人位置误差椭圆；
判断所述机器人位置误差椭圆的主轴长度是否大于预设搜索窗口尺寸初值；
若机器人位置误差椭圆的主轴长度大于预设搜索窗口初值，则使用机器人位置误差椭圆的主轴长度作为搜索窗口的尺寸；
若机器人位置误差椭圆的主轴长度小于或等于预设搜索窗口尺寸初值，则使用预设搜索窗口尺寸初值作为搜索窗口的尺寸。
11.根据权利要求10所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，当一次全局决策的扫描匹配完成后，将机器人位姿协方差矩阵重置为预设机器人位姿协方差矩阵初值，将搜索窗口的尺寸重置为预设搜索窗口尺寸初值。
12.一种基于多传感器决策融合的抗冲击扫描匹配系统，其特征在于，所述基于多传感器决策融合的抗冲击扫描匹配系统执行如权利要求1至权利要求11任意一项所述的基于多传感器决策融合的抗冲击扫描匹配方法，所述基于多传感器决策融合的抗冲击扫描匹配系统包括：
车轮编码器，用于检测机器人车轮的转速、位移信息；
陀螺仪，用于采集机器人角速度信息；
加速度计，用于采集机器人加速度信息；
激光雷达，用于采集激光点云数据；
数据处理单元，用于对车轮编码器、陀螺仪、加速度计等传感器测量值进行数据处理；
分布式检测单元，包括决策器一和决策器二，用于基于数据处理单元的数据处理结果生成一条以上的局部决策并传输至决策融合系统；
决策融合单元，用于接收分布式检测单元传输的一条以上的局部决策，结合决策融合规则生成全局决策；
扫描匹配单元，用于基于决策融合系统生成的全局决策在占用栅格地图中通过搜索窗口扫描匹配以获取占用栅格地图被激光点云占用概率最大的位姿。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110780758.1A CN113566828A (zh) | 2021-07-09 | 2021-07-09 | 一种基于多传感器决策融合的抗冲击扫描匹配方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110780758.1A CN113566828A (zh) | 2021-07-09 | 2021-07-09 | 一种基于多传感器决策融合的抗冲击扫描匹配方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113566828A true CN113566828A (zh) | 2021-10-29 |
Family
ID=78164401
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110780758.1A Pending CN113566828A (zh) | 2021-07-09 | 2021-07-09 | 一种基于多传感器决策融合的抗冲击扫描匹配方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113566828A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114440880A (zh) * | 2022-01-28 | 2022-05-06 | 山东省路桥集团有限公司 | 基于自适应迭代ekf的施工现场控制点定位方法及系统 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106123890A (zh) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | 一种多传感器数据融合的机器人定位方法 |
| CN107478214A (zh) * | 2017-07-24 | 2017-12-15 | 杨华军 | 一种基于多传感器融合的室内定位方法及系统 |
| US20180307941A1 (en) * | 2017-04-21 | 2018-10-25 | X Development Llc | Methods and Systems for Simultaneous Localization and Calibration |
| CN110285806A (zh) * | 2019-07-05 | 2019-09-27 | 电子科技大学 | 基于多次位姿校正的移动机器人快速精确定位算法 |
| CN111795686A (zh) * | 2020-06-08 | 2020-10-20 | 南京大学 | 一种移动机器人定位与建图的方法 |
| CN112612034A (zh) * | 2020-12-24 | 2021-04-06 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 基于激光帧和概率地图扫描的位姿匹配方法 |
| CN113029138A (zh) * | 2021-04-02 | 2021-06-25 | 扬州大学 | 一种基于多传感器数据融合的小车实时姿态检测方法 |
-
2021
- 2021-07-09 CN CN202110780758.1A patent/CN113566828A/zh active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106123890A (zh) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | 一种多传感器数据融合的机器人定位方法 |
| US20180307941A1 (en) * | 2017-04-21 | 2018-10-25 | X Development Llc | Methods and Systems for Simultaneous Localization and Calibration |
| CN107478214A (zh) * | 2017-07-24 | 2017-12-15 | 杨华军 | 一种基于多传感器融合的室内定位方法及系统 |
| CN110285806A (zh) * | 2019-07-05 | 2019-09-27 | 电子科技大学 | 基于多次位姿校正的移动机器人快速精确定位算法 |
| CN111795686A (zh) * | 2020-06-08 | 2020-10-20 | 南京大学 | 一种移动机器人定位与建图的方法 |
| CN112612034A (zh) * | 2020-12-24 | 2021-04-06 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 基于激光帧和概率地图扫描的位姿匹配方法 |
| CN113029138A (zh) * | 2021-04-02 | 2021-06-25 | 扬州大学 | 一种基于多传感器数据融合的小车实时姿态检测方法 |
Non-Patent Citations (1)
| Title |
|---|
| 吴敬阳: "基于激光雷达定位导航的多自主移动机器人系统研制", 《中国优秀硕士学位论文全文数据库信息科技辑》, pages 25 - 40 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114440880A (zh) * | 2022-01-28 | 2022-05-06 | 山东省路桥集团有限公司 | 基于自适应迭代ekf的施工现场控制点定位方法及系统 |
| CN114440880B (zh) * | 2022-01-28 | 2023-06-13 | 山东省路桥集团有限公司 | 基于自适应迭代ekf的施工现场控制点定位方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112985416B (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及系统 | |
| US20190065872A1 (en) | Behavior recognition apparatus, learning apparatus, and method and program therefor | |
| CN106814753B (zh) | 一种目标位置矫正方法、装置及系统 | |
| US9221170B2 (en) | Method and apparatus for controlling a robotic device via wearable sensors | |
| JP5459678B2 (ja) | 移動体画像追尾装置 | |
| CN113091738B (zh) | 基于视觉惯导融合的移动机器人地图构建方法及相关设备 | |
| CN107941212B (zh) | 一种视觉与惯性联合定位方法 | |
| CN112004183B (zh) | 一种基于卷积神经网络融合IMU和WiFi信息的机器人自主定位方法 | |
| US11080562B1 (en) | Key point recognition with uncertainty measurement | |
| EP3732549A1 (en) | Method for predicting a motion of an object, method for calibrating a motion model, method for deriving a predefined quantity and method for generating a virtual reality view | |
| CN113566828A (zh) | 一种基于多传感器决策融合的抗冲击扫描匹配方法及系统 | |
| CN113076988B (zh) | 基于神经网络的移动机器人视觉slam关键帧自适应筛选方法 | |
| KR20190081334A (ko) | 복합 측위 기반의 이동 궤적 추적 방법 및 그 장치 | |
| CN108801253A (zh) | 机器人建图定位系统及机器人 | |
| CN111693051B (zh) | 一种基于光电传感器的多目标数据关联方法 | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN109459046B (zh) | 悬浮式水下自主航行器的定位和导航方法 | |
| CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
| CN111735478B (zh) | 基于lstm的行人实时导航零速检测方法 | |
| CN113554705B (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
| US11079763B2 (en) | Trajectory planning method and system | |
| CN114485613A (zh) | 一种多信息融合水下机器人定位方法 | |
| Graovac | Principles of fusion of inertial navigation and dynamic vision | |
| CN115597575A (zh) | 一种机器人的侧滑检测方法、芯片及机器人 | |
| CN113793378B (zh) | 基于层次分组的语义slam对象关联和位姿更新方法及系统 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |