CN110986988B - 融合多传感器数据的轨迹推算方法、介质、终端和装置 - Google Patents
融合多传感器数据的轨迹推算方法、介质、终端和装置 Download PDFInfo
- Publication number
- CN110986988B CN110986988B CN201911329849.2A CN201911329849A CN110986988B CN 110986988 B CN110986988 B CN 110986988B CN 201911329849 A CN201911329849 A CN 201911329849A CN 110986988 B CN110986988 B CN 110986988B
- Authority
- CN
- China
- Prior art keywords
- robot
- time
- pose
- noise
- delta
- 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.)
- Active
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 26
- 230000003287 optical effect Effects 0.000 claims abstract description 54
- 238000000034 method Methods 0.000 claims abstract description 43
- 238000013519 translation Methods 0.000 claims abstract description 26
- 230000008859 change Effects 0.000 claims description 35
- 238000005259 measurement Methods 0.000 claims description 33
- 239000011159 matrix material Substances 0.000 claims description 26
- 238000004590 computer program Methods 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 13
- 238000001914 filtration Methods 0.000 claims description 9
- 238000010276 construction Methods 0.000 claims description 5
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 230000004927 fusion Effects 0.000 claims description 3
- 238000012935 Averaging Methods 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims 1
- 230000006870 function Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000012886 linear function Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
Abstract
本发明公开了一种融合多传感器数据的轨迹推算方法、介质、终端和装置，将编码器的数据作为机器人位姿状态的预测量，并将IMU的相对水平角度与光流的相对平移量作为观测数据更新机器人的位姿状态，不仅可以有效应对机器人轮子打滑导致的轨迹推算不准确状况，而且可以通过校正机器人的航向提高机器人位姿估计精度，从而适应更多的环境；同时本发明的模型简单实用，保证了机器人轨迹推算的实时性和高精度，具有较强的实际应用意义和商业价值。
Description
【技术领域】
本发明涉及机器人领域，尤其涉及一种融合多传感器数据的轨迹推算方法、介质、终端和装置。
【背景技术】
近些年来，随着机器人技术研究的日益成熟以及硬件条件的逐步改善，机器人实时轨迹的计算也有了更高的要求。在保证实时的前提下还要保证位姿的精度，这是机器人轨迹推算的一个基本问题。当前比较流行的方案是采用差分驱动轮作为机器人的主动轮，根据两个主动轮的转速之差进行轨迹推算。此方案具有实时性高计算量小的特点，且易于程序实现。在极低速的模式下，光流传感器也可以用来轨迹推算，其基于所采集的图像之间的纹理变化进行机器人位移的计算。在处理器性能比较好、光照条件比较好、地面纹理明显的情况下，基于光流的轨迹推算具有很高的精度。另外，惯性测量单元(IMU)可以提供角速度和加速度，因此也可以用来进行轨迹计算，其与编码器一样具有实时性高和计算量小的特点。上述三种方案是业界常用的选择，但是各自也存在一些缺陷：基于轮式编码器的轨迹推算方案，虽然具有实时性高和计算量小的特点，但是在轮子打滑的情况下，其计算结果明显是错误的；另外在机器人转弯时候，根据差分的原理所计算的相对偏角也是不准确的。对于应用光流的情况，其需要比较好的处理器性能，比较好的纹理变化和比较好的光照条件，而在室内场景中，光照条件是无法保证的。而对于IMU，由于零漂以及温漂的影响，其数据易受影响，从而导致所计算的轨迹具有非常大的累积误差。
【发明内容】
本发明提供了一种融合多传感器数据的轨迹推算方法、介质、终端和装置，解决了以上所述的技术问题。
本发明解决上述技术问题的技术方案如下：一种融合多传感器数据的轨迹推算方法，包括以下步骤：
步骤1，构造t-1和t时刻全局坐标系下机器人的位姿状态量；
步骤2，根据所述位姿状态量、t-1时刻以及t时刻机器人的编码器测量值生成t和t+1时刻机器人的位姿状态预测量；
步骤3，采用t到t+1时刻惯性测量单元的相对水平角度以及光流图像的相对平移量作为观测数据，并利用卡尔曼滤波方法对t+1时刻机器人的所述位姿状态预测量进行更新。
在一个优选实施方式中，所述步骤2中生成t和t+1时刻机器人的位姿状态预测量具体包括以下步骤：
S201，根据t-1时刻、t时刻以及t+1时刻机器人的编码器测量值以及里程计模型计算t-1到t时刻机器人的全局位姿变化量Δt以及t到t+1时刻机器人的全局位姿变化量Δt+1；
S202，生成t和t+1时刻机器人的位姿状态预测量如下：
(Xt,Xt+1)T＝(Xt-1,Xt)T+(Δt,Δt+1)T+w(t)，
其中，(Xt-1，Xt)T表示t-1和t时刻机器人的位姿状态量，(Xt，Xt+1)T表示t和t+1时刻机器人的位姿状态量，T为平移矩阵，w(t)为预测噪声。
在一个优选实施方式中，采用t到t+1时刻惯性测量单元的相对水平角度以及光流图像的相对平移量作为观测数据，并利用卡尔曼滤波方法对t+1时刻机器人的所述位姿状态预测量进行更新，具体包括以下步骤：
S301，生成所述位姿状态量在t+1时刻的协方差矩阵预测值，并根据所述协方差矩阵预测值计算卡尔曼增益；
S302，获取t和t+1时刻惯性测量单元输出的陀螺仪数据，对所述陀螺仪数据进行积分生成机器人在t和t+1时刻的相对水平角度；
S303，获取光流传感器输出的t到t+1时刻两帧光流图像的相对平移量；
S304，根据t到t+1时刻惯性测量单元的相对水平角度和光流图像的相对平移量生成机器人的全局位姿变化量δt，并建立观测模型如下：δt＝(Xt+1-Xt)T+v(t)，其中v(t)为观测噪声；
S305，根据所述卡尔曼增益和所述全局位姿变化量δt对所述协方差矩阵预测值和所述位姿状态预测量进行更新。
在一个优选实施方式中，对机器人所安装的编码器、惯性测量单元与光流传感器分别采集一组传感器数据，对每组传感器数据分别求取平均数生成不同传感器对应的平均噪声，将所述平均噪声乘以对应时间段的时间变化量生成对应传感器的噪声，其中，所述过程噪声为编码器噪声，观测噪声为惯性测量单元噪声和光流传感器噪声的组合。
本发明实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的融合多传感器数据的轨迹推算方法。
本发明实施例的第三方面提供了一种融合多传感器数据的轨迹推算终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述融合多传感器数据的轨迹推算方法的步骤。
本发明实施例的第四方面提供了一种融合多传感器数据的轨迹推算装置，所述轨迹推算装置包括状态量构造模块、预测量生成模块和状态量更新模块，
所述状态量构造模块用于构造t-1和t时刻全局坐标系下机器人的位姿状态量；
所述预测量生成模块用于根据所述位姿状态量、t-1时刻以及t时刻机器人的编码器测量值生成t和t+1时刻机器人的位姿状态预测量；
所述状态量更新模块用于采用t到t+1时刻惯性测量单元的相对水平角度以及光流图像的相对平移量作为观测数据，并利用卡尔曼滤波方法对t+1时刻机器人的所述位姿状态预测量进行更新。
在一个优选实施方式中，所述预测量生成模块具体包括：
第一计算单元，用于根据t-1时刻、t时刻以及t+1时刻机器人的编码器测量值以及里程计模型计算t-1到t时刻机器人的全局位姿变化量Δt以及t到t+1时刻机器人的全局位姿变化量Δt+1；
预测模型建立单元，用于生成t和t+1时刻机器人的位姿状态预测量如下：
(Xt,Xt+1)T＝(Xt-1,Xt)T+(Δt,Δt+1)T+w(t)，
其中，(Xt-1，Xt)T表示t-1和t时刻机器人的位姿状态量，(Xt，Xt+1)T表示t和t+1时刻机器人的位姿状态量，T为平移矩阵，w(t)为预测噪声。
在一个优选实施方式中，所述状态量更新模块具体包括：
第二计算单元，用于生成所述位姿状态量在t+1时刻的协方差矩阵预测值，并根据所述协方差矩阵预测值计算卡尔曼增益；
第三计算单元，用于获取t和t+1时刻惯性测量单元输出的陀螺仪数据，对所述陀螺仪数据进行积分生成机器人在t和t+1时刻的相对水平角度；
光流数据采集单元，用于获取光流传感器输出的t到t+1时刻两帧光流图像的相对平移量；
观测模型建立单元，用于根据t到t+1时刻惯性测量单元的相对水平角度和光流图像的相对平移量生成机器人的全局位姿变化量δt，并建立观测模型如下：δt＝(Xt+1-Xt)T+v(t)，其中v(t)为观测噪声；
更新单元，用于根据所述卡尔曼增益和所述全局位姿变化量δt对所述协方差矩阵预测值和所述位姿状态预测量进行更新。
在一个优选实施方式中，所述轨迹推算装置还包括噪声设定单元，所述噪声设定单元用于对机器人所安装的编码器、惯性测量单元与光流传感器分别采集一组传感器数据，对每组传感器数据分别求取平均数生成不同传感器对应的平均噪声，将所述平均噪声乘以对应时间段的时间变化量生成对应传感器的噪声，其中，所述过程噪声为编码器噪声，观测噪声为惯性测量单元噪声和光流传感器噪声的组合。
本发明提出了一种融合多传感器数据的轨迹推算方法、介质、终端和装置，将编码器的数据作为机器人位姿状态的预测量，并将IMU的相对水平角度与光流的相对平移量作为观测数据更新机器人的位姿状态，不仅可以有效应对机器人轮子打滑导致的轨迹推算不准确状况，而且可以通过校正机器人的航向提高机器人位姿估计精度，从而适应更多的环境；同时本发明的模型简单实用，保证了机器人轨迹推算的实时性和高精度，具有较强的实际应用意义和商业价值。
为使发明的上述目的、特征和优点能更明显易懂，下文特举本发明较佳实施例，并配合所附附图，作详细说明如下。
【附图说明】
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1是实施例1提供的融合多传感器数据的轨迹推算方法的流程示意图；
图2是实施例2提供的融合多传感器数据的轨迹推算装置的结构示意图；
图3是实施例3提供的融合多传感器数据的轨迹推算终端的结构示意图。
【具体实施方式】
为了使本发明的目的、技术方案和有益技术效果更加清晰明白，以下结合附图和具体实施方式，对本发明进行进一步详细说明。应当理解的是，本说明书中描述的具体实施方式仅仅是为了解释本发明，并不是为了限定本发明。
图1是本发明实施例1提供的一种融合多传感器数据的轨迹推算方法的流程示意图，如图1所示，包括以下步骤：
步骤1，构造t-1和t时刻全局坐标系下机器人的位姿状态量。
步骤2，根据所述位姿状态量、t-1时刻以及t时刻机器人的编码器测量值生成t和t+1时刻机器人的位姿状态预测量。
本发明由于观测量为IMU提供的相对水平角度和光流传感器提供的相对平移量，因此状态量取由编码器数据计算出的机器人在全局坐标系的相邻两个位姿，其中第t时刻位姿设定为Xt＝(xt,yt,θt)T，第0时刻机器人位姿设定为X0＝(0,0,0)T。具体来说，基于编码器的轨迹推算方法主要采用光电编码器在采样周期内脉冲的变化量计算出轮子相对于地面移动的距离和姿态角的变化量，从而推算出移动机器人位姿的相对变化。一个机器人在其轮子或腿关节处配备有光电编码器等设备，当它向前移动一段时间后，测量光电编码器脉冲数目，可以得出机器人轮子旋转的圈数，结合轮子的周长，便可以计算出机器人的移动距离。
本实施例将编码器安装在机器人两主动轮侧面从而读取转速信息；将惯性导航单元安装在两主动轮中心点，即机器人的自转圆心的正上方，读取陀螺仪信息；光流传感器安装在机器人底部朝向地面，同时增加一个LED灯以使得获取的地面图像比较清晰。然后根据t-1时刻、t时刻、t+1时刻机器人的编码器测量值以及里程计模型计算t-1到t时刻机器人的全局位姿变化量Δt以及t到t+1时刻机器人的全局位姿变化量Δt+1。具体来说，本实施例中设定机器人为差速驱动，在模型预测阶段，设t-1到t时刻编码器左右轮位移变化量分别为ΔSl，ΔSr，得到机器人参考点位置变化量为ΔSt＝(ΔSl+ΔSr)/2，姿态角变化量为Δθt＝(ΔSr-ΔSl)/(2L)，其中2L表示差速驱动模型的两驱动轮的间距。将变化量分解到全局坐标系下，分别得到t-1到t时刻沿两坐标轴的位置变化量Δxt＝ΔStcos(θt+Δθt/2)，Δyt＝ΔStsin(θt+Δθt/2)，最后即可推算出t-1到t时刻机器人位姿全局变化量Δt＝(Δxt,Δyt,Δθt)T。同理可以得到Δt+1。
然后生成t和t+1时刻机器人的位姿状态预测量如下：
(Xt,Xt+1)T＝(Xt-1,Xt)T+(Δt,Δt+1)T+w(t)，
其中，(Xt-1，Xt)T表示t-1和t时刻机器人的位姿状态量，(Xt，Xt+1)T表示t和t+1时刻机器人的位姿状态量，T为平移矩阵，w(t)为预测噪声。
然后进入模型观测更新阶段：采用t到t+1时刻惯性测量单元的相对水平角度以及光流图像的相对平移量作为观测数据，并利用卡尔曼滤波方法对t+1时刻机器人的所述位姿状态预测量进行更新。具体包括以下步骤：
S301，生成所述位姿状态量在t+1时刻的协方差矩阵预测值，并根据所述协方差矩阵预测值计算卡尔曼增益，所述协方差矩阵为时间间隔的一次函数。
S302，获取t和t+1时刻惯性测量单元输出的陀螺仪数据，对所述陀螺仪数据进行积分生成机器人在t和t+1时刻的相对水平角度，具体相对角度的解算过程已经过滤波处理。
S303，获取光流传感器输出的t到t+1时刻两帧光流图像的相对平移量，所述光流传感器可以采用基于视觉SIFT特征点的光流追踪方法、基于网格化的光流跟踪方法等等进行特征跟踪，在应用光流数据进行相对位移解算时，需要一个放缩因子，该因子可在前期对光流数据进行标定时得到。
S304，根据t到t+1时刻惯性测量单元的相对水平角度和光流图像的相对平移量生成机器人的全局位姿变化量δt，并建立观测模型如下：δt＝(Xt+1-Xt)T+v(t)，其中v(t)为观测噪声。所述观测模型含有两个传感器的数据，其协方差也是随时间变化的，仍然定义该协方差是时间间隔的一次函数。并且本发明设定光流传感器和惯性测量单元是互不影响的，因此该协方差是一个对角矩阵。
S305，根据所述卡尔曼增益和所述全局位姿变化量δt对所述协方差矩阵预测值和所述位姿状态预测量进行更新。由于各个传感器传出数据的时间是不一致的，因此在观测模型的更新步骤时只有传感器数据到来时才进行更新，若数据没到达则仍然继续执行状态预测步骤。
同时，对于预测阶段与观测阶段的噪声，首先对机器人所安装的编码器、惯性测量单元与光流传感器分别采集一组传感器数据，对每组传感器数据分别求取平均数生成不同传感器对应的平均噪声，将所述平均噪声乘以对应时间段的时间变化量生成对应传感器的噪声，从而确定预测阶段的过程噪声和观测阶段的观测噪声，所述过程噪声为编码器噪声，观测噪声为惯性测量单元噪声和光流传感器噪声的组合，即惯性测量单元噪声和光流噪声都是协方差矩阵的元素。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
本发明实施例还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的融合多传感器数据的轨迹推算方法。
图2是本发明实施例2提供的一种融合多传感器数据的轨迹推算装置的结构示意图，如图2所示，包括状态量构造模块100、预测量生成模块200和状态量更新模块300，
所述状态量构造模块100用于构造相邻的t-1和t时刻全局坐标系下机器人的位姿状态量；
所述预测量生成模块200用于根据所述位姿状态量、t-1时刻以及t时刻机器人的编码器测量值生成t和t+1时刻机器人的位姿状态预测量；
所述状态量更新模块300用于采用t到t+1时刻惯性测量单元的相对水平角度以及光流图像的相对平移量作为观测数据，并利用卡尔曼滤波方法对t+1时刻机器人的所述位姿状态预测量进行更新。
在一个优选实施方式中，所述预测量生成模块200具体包括：
第一计算单元201，用于根据t-1时刻、t时刻以及t+1时刻机器人的编码器测量值以及里程计模型计算t-1到t时刻机器人的全局位姿变化量Δt以及t到t+1时刻机器人的全局位姿变化量Δt+1；
预测模型建立单元202，用于生成t和t+1时刻机器人的位姿状态预测量如下：
(Xt,Xt+1)T＝(Xt-1,Xt)T+(Δt,Δt+1)T+w(t)，
其中，(Xt-1，Xt)T表示t-1和t时刻机器人的位姿状态量，(Xt，Xt+1)T表示t和t+1时刻机器人的位姿状态量，T为平移矩阵，w(t)为预测噪声。
在一个优选实施方式中，所述状态量更新模块300具体包括：
第二计算单元301，用于生成所述位姿状态量在t+1时刻的协方差矩阵预测值，并根据所述协方差矩阵预测值计算卡尔曼增益；
第三计算单元302，用于获取t和t+1时刻惯性测量单元输出的陀螺仪数据，对所述陀螺仪数据进行积分生成机器人在t和t+1时刻的相对水平角度；
光流数据采集单元303，用于获取光流传感器输出的t到t+1时刻两帧光流图像的相对平移量；
观测模型建立单元304，用于根据t到t+1时刻惯性测量单元的相对水平角度和光流图像的相对平移量生成机器人的全局位姿变化量δt，并建立观测模型如下：δt＝(Xt+1-Xt)T+v(t)，其中v(t)为观测噪声；
更新单元305，用于根据所述卡尔曼增益和所述全局位姿变化量δt对所述协方差矩阵预测值和所述位姿状态预测量进行更新。
在一个优选实施方式中，所述轨迹推算装置还包括噪声设定单元400，所述噪声设定单元400用于对机器人所安装的编码器、惯性测量单元与光流传感器分别采集一组传感器数据，对每组传感器数据分别求取平均数生成不同传感器对应的平均噪声，将所述平均噪声乘以对应时间段的时间变化量生成对应传感器的噪声。
本发明实施例还提供了一种融合多传感器数据的轨迹推算终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述融合多传感器数据的轨迹推算方法的步骤。图3是本发明实施例3提供的融合多传感器数据的轨迹推算终端的结构示意图，如图3所示，该实施例的融合多传感器数据的轨迹推算终端8包括：处理器80、可读存储介质81以及存储在所述可读存储介质81中并可在所述处理器80上运行的计算机程序82。所述处理器80执行所述计算机程序82时实现上述各个方法实施例中的步骤，例如图1所示的步骤1至步骤3。或者，所述处理器80执行所述计算机程序82时实现上述各装置实施例中各模块的功能，例如图2所示模块100至300的功能。
示例性的，所述计算机程序82可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述可读存储介质81中，并由所述处理器80执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序82在所述融合多传感器数据的轨迹推算终端8中的执行过程。
所述融合多传感器数据的轨迹推算终端8可包括，但不仅限于，处理器80、可读存储介质81。本领域技术人员可以理解，图3仅仅是融合多传感器数据的轨迹推算终端8的示例，并不构成对融合多传感器数据的轨迹推算终端8的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述融合多传感器数据的轨迹推算终端还可以包括电源管理模块、运算处理模块、输入输出设备、网络接入设备、总线等。
所称处理器80可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述可读存储介质81可以是所述融合多传感器数据的轨迹推算终端8的内部存储单元，例如融合多传感器数据的轨迹推算终端8的硬盘或内存。所述可读存储介质81也可以是所述融合多传感器数据的轨迹推算终端8的外部存储设备，例如所述融合多传感器数据的轨迹推算终端8上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述可读存储介质81还可以既包括所述融合多传感器数据的轨迹推算终端8的内部存储单元也包括外部存储设备。所述可读存储介质81用于存储所述计算机程序以及所述融合多传感器数据的轨迹推算终端所需的其他程序和数据。所述可读存储介质81还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及方法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
本发明并不仅仅限于说明书和实施方式中所描述，因此对于熟悉领域的人员而言可容易地实现另外的优点和修改，故在不背离权利要求及等同范围所限定的一般概念的精神和范围的情况下，本发明并不限于特定的细节、代表性的设备和这里示出与描述的图示示例。
Claims (6)
1.一种融合多传感器数据的轨迹推算方法，其特征在于，包括以下步骤：
步骤1，构造t-1和t时刻全局坐标系下机器人的位姿状态量；
步骤2，根据所述位姿状态量、t-1时刻以及t时刻机器人的编码器测量值生成t和t+1时刻机器人的位姿状态预测量；
步骤3，采用t到t+1时刻惯性测量单元的相对水平角度以及光流图像的相对平移量作为观测数据，并利用卡尔曼滤波方法对t+1时刻机器人的所述位姿状态预测量进行更新；
所述步骤2中生成t和t+1时刻机器人的位姿状态预测量具体包括以下步骤：
S201，根据t-1时刻、t时刻以及t+1时刻机器人的编码器测量值以及里程计模型计算t-1到t时刻机器人的全局位姿变化量Δt以及t到t+1时刻机器人的全局位姿变化量Δt+1；
S202，生成t和t+1时刻机器人的位姿状态预测量如下：
(Xt,Xt+1)T＝(Xt-1,Xt)T+(Δt,Δt+1)T+w(t)，
其中，(Xt-1,Xt)T表示t-1和t时刻机器人的位姿状态量，(Xt,Xt+1)T表示t和t+1时刻机器人的位姿状态量，T为向量转置，w(t)为过程噪声；
步骤S201中，Δt的计算方法为：设t-1到t时刻编码器左右轮位移变化量分别为ΔSl，ΔSr，得到机器人参考点位置变化量为ΔSt＝(ΔSl+ΔSr)/2，姿态角变化量为Δθt＝(ΔSr-ΔSl)/(2L)，其中2L表示差速驱动模型的两驱动轮的间距；将变化量分解到全局坐标系下，分别得到t-1到t时刻沿两坐标轴的位置变化量Δxt＝ΔStcos(θt+Δθt/2)，Δyt＝ΔStsin(θt+Δθt/2)，最后即可推算出t-1到t时刻机器人位姿全局变化量Δt＝(Δxt,Δyt,Δθt)T；
采用t到t+1时刻惯性测量单元的相对水平角度以及光流图像的相对平移量作为观测数据，并利用卡尔曼滤波方法对t+1时刻机器人的所述位姿状态预测量进行更新，具体包括以下步骤：
S301，生成所述位姿状态量在t+1时刻的协方差矩阵预测值，并根据所述协方差矩阵预测值计算卡尔曼增益；
S302，获取t和t+1时刻惯性测量单元输出的陀螺仪数据，对所述陀螺仪数据进行积分生成机器人在t和t+1时刻的相对水平角度；
S303，获取光流传感器输出的t到t+1时刻两帧光流图像的相对平移量；
S304，根据t到t+1时刻惯性测量单元的相对水平角度和光流图像的相对平移量生成机器人的全局位姿变化量δt，并建立观测模型如下：δt＝(Xt+1-Xt)T+v(t)，其中v(t)为观测噪声；
S305，根据所述卡尔曼增益和所述全局位姿变化量δt对所述协方差矩阵预测值和所述位姿状态预测量进行更新。
2.根据权利要求1所述融合多传感器数据的轨迹推算方法，其特征在于，对机器人所安装的编码器、惯性测量单元与光流传感器分别采集一组传感器数据，对每组传感器数据分别求取平均数生成不同传感器对应的平均噪声，将所述平均噪声乘以对应时间段的时间变化量生成对应传感器的噪声，其中，所述过程噪声为编码器噪声，观测噪声为惯性测量单元噪声和光流传感器噪声的组合。
3.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1-2任一项所述融合多传感器数据的轨迹推算方法。
4.一种融合多传感器数据的轨迹推算终端，其特征在于，包括权利要求3所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现如权利要求1-2任一项所述融合多传感器数据的轨迹推算方法的步骤。
5.一种融合多传感器数据的轨迹推算装置，其特征在于，所述轨迹推算装置包括状态量构造模块、预测量生成模块和状态量更新模块，
所述状态量构造模块用于构造t-1和t时刻全局坐标系下机器人的位姿状态量；
所述预测量生成模块用于根据所述位姿状态量、t-1时刻以及t时刻机器人的编码器测量值生成t和t+1时刻机器人的位姿状态预测量；
所述状态量更新模块用于采用t到t+1时刻惯性测量单元的相对水平角度以及光流图像的相对平移量作为观测数据，并利用卡尔曼滤波方法对t+1时刻机器人的所述位姿状态预测量进行更新；
所述预测量生成模块具体包括：
第一计算单元，用于根据t-1时刻、t时刻以及t+1时刻机器人的编码器测量值以及里程计模型计算t-1到t时刻机器人的全局位姿变化量Δt以及t到t+1时刻机器人的全局位姿变化量Δt+1；
预测模型建立单元，用于生成t和t+1时刻机器人的位姿状态预测量如下：
(Xt,Xt+1)T＝(Xt-1,Xt)T+(Δt,Δt+1)T+w(t)，
其中，(Xt-1,Xt)T表示t-1和t时刻机器人的位姿状态量，(Xt,Xt+1)T表示t和t+1时刻机器人的位姿状态量，T为平移矩阵，w(t)为过程噪声；
其中，Δt的计算方法为：设t-1到t时刻编码器左右轮位移变化量分别为ΔSl，ΔSr，得到机器人参考点位置变化量为ΔSt＝(ΔSl+ΔSr)/2，姿态角变化量为Δθt＝(ΔSr-ΔSl)/(2L)，其中2L表示差速驱动模型的两驱动轮的间距；将变化量分解到全局坐标系下，分别得到t-1到t时刻沿两坐标轴的位置变化量Δxt＝ΔStcos(θt+Δθt/2)，Δyt＝ΔStsin(θt+Δθt/2)，最后即可推算出t-1到t时刻机器人位姿全局变化量Δt＝(Δxt,Δyt,Δθt)T；
所述状态量更新模块具体包括：
第二计算单元，用于生成所述位姿状态量在t+1时刻的协方差矩阵预测值，并根据所述协方差矩阵预测值计算卡尔曼增益；
第三计算单元，用于获取t和t+1时刻惯性测量单元输出的陀螺仪数据，对所述陀螺仪数据进行积分生成机器人在t和t+1时刻的相对水平角度；
光流数据采集单元，用于获取光流传感器输出的t到t+1时刻两帧光流图像的相对平移量；
观测模型建立单元，用于根据t到t+1时刻惯性测量单元的相对水平角度和光流图像的相对平移量生成机器人的全局位姿变化量δt，并建立观测模型如下：δt＝(Xt+1-Xt)T+v(t)，其中v(t)为观测噪声；
更新单元，用于根据所述卡尔曼增益和所述全局位姿变化量δt对所述协方差矩阵预测值和所述位姿状态预测量进行更新。
6.根据权利要求5所述融合多传感器数据的轨迹推算装置，其特征在于，所述轨迹推算装置还包括噪声设定单元，所述噪声设定单元用于对机器人所安装的编码器、惯性测量单元与光流传感器分别采集一组传感器数据，对每组传感器数据分别求取平均数生成不同传感器对应的平均噪声，将所述平均噪声乘以对应时间段的时间变化量生成对应传感器的噪声，其中，所述过程噪声为编码器噪声，观测噪声为惯性测量单元噪声和光流传感器噪声的组合。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911329849.2A CN110986988B (zh) | 2019-12-20 | 2019-12-20 | 融合多传感器数据的轨迹推算方法、介质、终端和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911329849.2A CN110986988B (zh) | 2019-12-20 | 2019-12-20 | 融合多传感器数据的轨迹推算方法、介质、终端和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110986988A CN110986988A (zh) | 2020-04-10 |
| CN110986988B true CN110986988B (zh) | 2023-12-08 |
Family
ID=70073767
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911329849.2A Active CN110986988B (zh) | 2019-12-20 | 2019-12-20 | 融合多传感器数据的轨迹推算方法、介质、终端和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110986988B (zh) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112150550B (zh) * | 2020-09-23 | 2021-07-27 | 华人运通(上海)自动驾驶科技有限公司 | 一种融合定位方法及装置 |
| CN112535434B (zh) * | 2020-10-23 | 2022-01-11 | 湖南新视电子技术有限公司 | 一种无尘室智能扫地机器人 |
| CN113128607B (zh) * | 2021-04-25 | 2023-10-10 | 思创数码科技股份有限公司 | 一种轨迹相似度计算方法 |
| CN113432586A (zh) * | 2021-06-24 | 2021-09-24 | 国网浙江省电力有限公司双创中心 | 地下管道巡检设备的轨迹测绘方法及地下管道巡检设备 |
| CN114147717A (zh) * | 2021-12-09 | 2022-03-08 | 乐聚(深圳)机器人技术有限公司 | 机器人运动轨迹估计方法、装置、控制器及存储介质 |
| CN114355920B (zh) * | 2021-12-27 | 2024-02-02 | 深圳银星智能集团股份有限公司 | 行进方向的控制方法、装置、智能设备及存储介质 |
| CN114543797A (zh) * | 2022-02-18 | 2022-05-27 | 北京市商汤科技开发有限公司 | 位姿预测方法和装置、设备、介质 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104819716A (zh) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | 一种基于mems的ins/gps组合的室内外个人导航算法 |
| CN107478214A (zh) * | 2017-07-24 | 2017-12-15 | 杨华军 | 一种基于多传感器融合的室内定位方法及系统 |
| CN107490373A (zh) * | 2017-08-25 | 2017-12-19 | 杭州德泽机器人科技有限公司 | 基于编码器和惯性元器件融合的设备位姿估计方法及系统 |
| CN108375370A (zh) * | 2018-07-02 | 2018-08-07 | 江苏中科院智能科学技术应用研究院 | 一种面向智能巡防无人机的复合导航系统 |
| CN108827318A (zh) * | 2018-08-20 | 2018-11-16 | 中科物栖（北京）科技有限责任公司 | 无人机室内定位方法及装置 |
| CN109141411A (zh) * | 2018-07-27 | 2019-01-04 | 顺丰科技有限公司 | 定位方法、定位装置、移动机器人及存储介质 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150219767A1 (en) * | 2014-02-03 | 2015-08-06 | Board Of Regents, The University Of Texas System | System and method for using global navigation satellite system (gnss) navigation and visual navigation to recover absolute position and attitude without any prior association of visual features with known coordinates |
| US9709404B2 (en) * | 2015-04-17 | 2017-07-18 | Regents Of The University Of Minnesota | Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation |
-
2019
- 2019-12-20 CN CN201911329849.2A patent/CN110986988B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104819716A (zh) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | 一种基于mems的ins/gps组合的室内外个人导航算法 |
| CN107478214A (zh) * | 2017-07-24 | 2017-12-15 | 杨华军 | 一种基于多传感器融合的室内定位方法及系统 |
| CN107490373A (zh) * | 2017-08-25 | 2017-12-19 | 杭州德泽机器人科技有限公司 | 基于编码器和惯性元器件融合的设备位姿估计方法及系统 |
| CN108375370A (zh) * | 2018-07-02 | 2018-08-07 | 江苏中科院智能科学技术应用研究院 | 一种面向智能巡防无人机的复合导航系统 |
| CN109141411A (zh) * | 2018-07-27 | 2019-01-04 | 顺丰科技有限公司 | 定位方法、定位装置、移动机器人及存储介质 |
| CN108827318A (zh) * | 2018-08-20 | 2018-11-16 | 中科物栖（北京）科技有限责任公司 | 无人机室内定位方法及装置 |
Non-Patent Citations (2)
| Title |
|---|
| Fusion of visual and inertial measurement information for unmanned aerial vehicles autonomous navigation in unknown environment;Li Jianguo等;《 2017 20th International Conference on Information Fusion (Fusion)》;全文 * |
| 多传感器融合实现机器人精确定位;吴伟;刘兴刚;王忠实;徐心和;;东北大学学报(自然科学版)(第02期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110986988A (zh) | 2020-04-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110986988B (zh) | 融合多传感器数据的轨迹推算方法、介质、终端和装置 | |
| CN111811506B (zh) | 视觉/惯性里程计组合导航方法、电子设备及存储介质 | |
| CN110160542B (zh) | 车道线的定位方法和装置、存储介质、电子装置 | |
| CN109084732B (zh) | 定位与导航方法、装置及处理设备 | |
| CN107941217B (zh) | 一种机器人定位方法、电子设备、存储介质、装置 | |
| CN109933056B (zh) | 一种基于slam的机器人导航方法以及机器人 | |
| KR102570094B1 (ko) | 측위 방법과 장치, 자율 주행 차량, 전자기기와 저장매체 | |
| JP2021177168A (ja) | 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム | |
| CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
| CN114612348B (zh) | 激光点云运动畸变校正方法、装置、电子设备及存储介质 | |
| CN108759822B (zh) | 一种移动机器人3d定位系统 | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| CN103712598A (zh) | 一种小型无人机姿态确定系统与确定方法 | |
| CN110887493A (zh) | 基于局部地图匹配的轨迹推算方法、介质、终端和装置 | |
| CN104913775A (zh) | 无人机对地高度的测量方法、无人机定位方法及装置 | |
| Liu et al. | Bidirectional trajectory computation for odometer-aided visual-inertial SLAM | |
| CN114323033A (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
| CN111721305A (zh) | 定位方法和装置、自动驾驶车辆、电子设备和存储介质 | |
| CN113763504A (zh) | 地图更新方法、系统、车载终端、服务器及存储介质 | |
| CN209764110U (zh) | 融合图像传感器和惯性传感器的智能小车控制系统 | |
| Zhou et al. | An embedded solution to visual mapping for consumer drones | |
| US20240077880A1 (en) | Slope location correction method and apparatus, robot and readable storage medium | |
| CN115560744A (zh) | 机器人以及基于多传感器的三维建图方法、存储介质 | |
| CN115585805A (zh) | 车辆定位方法、定位装置、定位系统以及车辆 | |
| WO2022237375A1 (zh) | 用于定位装置和里程计的标定方法、程序产品和标定装置 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |