CN110645976A - 一种移动机器人的姿态估计方法及终端设备 - Google Patents

Abstract

本发明提供一种移动机器人的姿态估计方法，所述移动机器人包括车体，其中所述车体上固定安装有IMU和里程计，该方法包括：对所述IMU所采集的IMU数据进行互补滤波，以估计所述移动机器人的运动姿态，其中所述运动姿态包括偏航角、俯仰角和横滚角；以及利用所述里程计的更新周期和所述里程计所采集的里程计数据校正所述运动姿态中的偏航角，并将校正后的偏航角更新为所述运动姿态中的偏航角。本发明还提供相应的终端设备。本发明能够准确估计每个姿态中的偏航角。

Description

一种移动机器人的姿态估计方法及终端设备

技术领域

本发明的所公开实施例涉及移动机器人导航定位技术领域，且更具体而言，涉及一种移动机器人的姿态估计方法及终端设备。

背景技术

在移动机器人的应用中，导航定位是其应用的关键技术，导航需要精准的定位。目前姿态估计的方法中，包括基于视觉传感器、GPS等。基于视觉传感器的方法对周围环境表面纹理和光照要求较高；基于激光雷达的方法对周围环境结构特征要求较高，而GPS在遮挡区域和室内无法应用，只能在开阔的室外环境中使用。

发明内容

根据本发明的实施例，本发明提出一种移动机器人的姿态估计方法及终端设备，以解决上述问题。

根据本发明的第一方面，公开一种实例性的一种移动机器人的姿态估计方法，所述移动机器人包括车体，其中所述车体上固定安装有IMU和里程计，该方法包括：对所述IMU所采集的IMU数据进行互补滤波，以估计所述移动机器人的运动姿态，其中所述运动姿态包括偏航角、俯仰角和横滚角；以及利用所述里程计的更新周期和所述里程计所采集的里程计数据校正所述运动姿态中的偏航角，并将校正后的偏航角更新为所述运动姿态中的偏航角。

根据本发明的第二方面，公开一种实例性的终端设备，该终端设备包括处理器和存储器，存储器存储有指令，指令在执行时使得处理器执行上述第一方面的移动机器人的姿态估计方法。

本发明的有益效果有：通过对IMU所采集的IMU数据进行互补滤波，估计移动机器人的运动姿态，能够准确估计每个姿态中的俯仰角和横滚角，进而利用里程计的更新周期和里程计所采集的里程计数据校正运动姿态中的偏航角，能够准确估计每个姿态中的偏航角，使得偏航角的估计更准确。

附图说明

图1是为本发明实施例的移动机器人的结构示意图。

图2是本发明第一实施例的移动机器人的姿态估计方法的流程图。

图3是本发明第二实施例的移动机器人的姿态估计方法的流程图。

图4是本发明第三实施例的移动机器人的姿态估计方法的流程图。

图5是本发明实施例的终端设备的结构示意图。

图6是本发明实施例的一种非易失性存储介质的示意图。

具体实施方式

为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施方式对本发明的技术方案做进一步详细描述。

为了清楚理解本发明，下面先对移动机器人进行说明。

如图1所示，为本发明实施例的移动机器人的结构示意图，该移动机器人100可以包括轮式机器人。该移动机器人100包括车体101，车体101上固定安装有IMU(Inertialmeasurement unit，惯导)102和里程计(odometry)103。IMU 102包括三轴陀螺仪和三轴加速度计。IMU 102和里程计103在车体上的位置以实际应用为准，此处不作限定。由于IMU102和里程计103固定安装在车体101上，假定IMU 102的坐标系和里程计103的坐标系与移动机器人的载体坐标系一致。下面对移动机器人100的坐标系进行说明。

移动机器人100的坐标系包括载体坐标系和导航坐标系。载体坐标系标记为b坐标系，其中X^b轴指向车体前向，Y^b轴指向车体左侧向，Z^b轴垂直于车体横切面指向上方，三个轴向符合右手定则，原点O^b为车体中心。导航坐标系标记为n坐标系，其中XⁿOⁿYⁿ平面与水平面平行，Xⁿ轴指向与初始时刻的b坐标系的X^b轴在水平面的投影一致，Zⁿ轴指向天向，Yⁿ轴方向由右手定则确定，原点Oⁿ为初始时刻的b坐标系的原点O^b。

下面结合图1中的移动机器人说明本发明的技术方案。

如图2所示，为本发明第一实施例的移动机器人的姿态估计方法的流程图，该方法可由终端设备执行，例如移动终端、台式计算机、服务器等，该方法包括：

步骤110：对IMU所采集的IMU数据进行互补滤波，以估计移动机器人的运动姿态。

其中，运动姿态包括偏航角ψ、俯仰角θ和横滚角γ。

IMU包括三轴陀螺仪和三轴加速度计，则IMU数据包括三轴陀螺仪所采集的陀螺仪数据和三轴加速度计所采集的加速度计数据。

对不同时刻采集的IMU数据进行互补滤波可以估计出移动机器人在相应时刻的姿态，进而不同时刻的姿态形成运动姿态。估计的不同时刻的姿态可以进行存储，例如，可以存储到一存储器中。

其中，在一些实施例中，在互补滤波之前，对移动机器人进行初始化操作，以估计出移动机器人的初始姿态。移动机器人的初始姿态指移动机器人处于静止状态时的姿态，且由加速度计数据估计。静止状态时的加速度计数据包括加速度矢量a＝[a_bx a_by a_bz]^T，初始姿态包括偏航角ψ₀、俯仰角θ₀和横滚角γ₀。由于移动机器人处于静止状态，三轴加速度计所测量的数据为重力加速度矢量的模值，则偏航角ψ₀、俯仰角θ₀和横滚角γ₀的计算公式为如下：

ψ₀＝0

其中，g为重力加速度矢量的模值。

步骤120：利用里程计的更新周期和里程计所采集的里程计数据校正运动姿态中的偏航角，并将校正后的偏航角更新为运动姿态中的偏航角。

里程计数据包括里程计角速度。里程计的更新周期是指里程计数据的更新周期。

利用里程计的更新周期和里程计数据校正运动姿态中的偏航角，进而将校正后的偏航角作为运动姿态中的偏航角。例如，利用里程计的更新周期和某个时刻的里程计数据校正该时刻的姿态中的偏航角，进而将校正后的偏航角作为该时刻的姿态中的偏航角。

在本实施例中，通过对IMU所采集的IMU数据进行互补滤波，估计移动机器人的运动姿态，能够准确估计每个姿态中的俯仰角和横滚角，进而利用里程计的更新周期和里程计所采集的里程计数据校正运动姿态中的偏航角，能够准确估计每个姿态中的偏航角，使得偏航角的估计更准确。

在一些实施例中，里程计所采集的里程计数据包括当前时刻t(k)的里程计角速度w′_bz(k)和更新时刻t(k-m)的里程计角速度w′_bz(k-m)。其中更新时刻t(k-m)是指里程计数据更新的时间点。

如图3所示，为本发明第二实施例的移动机器人的姿态估计方法的流程图，在图1中的第一实施例的基础上，上述步骤120包括：

步骤1201：获取通过互补滤波所估计的当前时刻t(k)的偏航角ψ_k和更新时刻t(k-m)的偏航角ψ_k-m，并根据公式1，计算更新时刻t(k-m)至当前时刻t(k)的第一偏航角增量

其中，里程计的更新周期为mΔT。

如上所述，通过互补滤波所估计的运动姿态存储在存储器中，则可以从存储器中获取当前时刻t(k)的偏航角ψ_k和更新时刻t(k-m)的偏航角ψ_k-m。

步骤1202：根据当前时刻t(k)的里程计角速度w′_bz(k)和更新时刻t(k-m)的里程计角速度w′_bz(k-m)，计算更新时刻t(k-m)至当前时刻t(k)的第二偏航角增量

即第二偏航角增量

是里程计数据计算而来。

步骤1203：确定第二偏航角增量

的加权值α。

其中，0≤α≤1。

步骤1204：根据公式2，计算最终偏航角增量

即对第一偏航角增量

和第二偏航角增量

进行加权平均得到最终偏航角增量

步骤1205：根据公式3，计算当前时刻t(k)的校正后的偏航角ψ’_k。

将最终偏航角增量

与更新时刻t(k-m)的偏航角ψ_k-m相加得到当前时刻t(k)的校正后的偏航角ψ_k。校正后的偏航角ψ’_k替代互补滤波所估计的当前时刻t(k)的偏航角ψ_k，作为当前时刻t(k)的偏航角ψ_k，即ψ_k＝ψ’_k。

本实施例中，通过对IMU数据进行互补滤波所估计得到的第一偏航角增量和根据里程计角速度所计算得到的第二偏航角增量进行加权融合，得到最终偏航角增量，并利用最终偏航角增量来计算校正后的偏航角，进而实现校正互补滤波所估计的偏航角，使得偏航角的估计结果更准确。

在一些实施例中，上述步骤1202中，根据当前时刻t(k)的里程计角速度w′_bz(k)和更新时刻t(k-m)的里程计角速度w′_bz(k-m)，计算所述更新时刻t(k-m)至所述当前时刻t(k)的第二偏航角增量

具体如下：

先根据公式4，分别计算当前时刻t(k)的里程计角速度w′_bz(k)在导航坐标系Z轴的分量w′_nz(k)和更新时刻t(k-m)的里程计角速度w′_bz(k-m)在导航坐标系Z轴的分量w′_nz(k-m)，

其中，

分别为时刻t(k_i)时移动机器人的俯仰角和横滚角。

随后，根据公式5，计算角速度

其中，λ₀为根据里程计的更新频率而变化的参数，且0≤λ₀≤1。其中，在一些实施例中，该更新频率越小则λ₀越大。

可以看出，角速度

是当前时刻t(k)的里程计角速度w′_bz(k)在导航坐标系Z轴的分量w′_nz(k)和更新时刻t(k-m)的里程计角速度w′_bz(k-m)在导航坐标系Z轴的分量w′_nz(k-m)加权平均得到的。

再随后，根据公式6，计算第二偏航角增量

可以看到，第二偏航角增量

是对角速度

进行积分运算得到的。

在一些实施例中，IMU数据包括陀螺仪数据和加速度计数据，陀螺仪数据包括当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k) w_bz(k)]^T。

在一些实施例中，上述步骤1203中，确定第二偏航角增量

的加权值α。具体如下：

先根据公式7，估计移动机器人沿载体坐标系Z轴的角速度的绝对值

其中，w_bz表示陀螺仪角速度矢量w(k)在载体坐标系Z轴上分量的值，m′为可调节的参数，满足0<m′≤m。

随后，根据里程计角速度范围的阈值ω_E确定加权值α，其中，ω_E>0，当

时，加权值α大于预设值，当

时，加权值α小于该预设值，且该预设值处于0到1之间。可以看出，当

时，加权值α较大，当

时，加权值α较小。

可以看出，移动机器人沿载体坐标系Z轴的角速度的绝对值与里程计角速度范围的阈值ω_E进行比较，确定第二偏航角增量

的加权值α。

本实施例中，通过陀螺仪数据和里程计数据的比较，实现自适应确定由互补滤波所估计得到的第二偏航角增量

的加权值α，确保陀螺仪数据的测量误差小时，更信任使用对IMU数据进行互补滤波所估计得到的偏航角增量，里程计角速度的测量误差小时，更信任使用由里程计角速度数据计算得到的偏航角增量，进而使得偏航角的估计在车体不同运动状态下均具备较好的效果。

在一些实施例中，加权值α是根据公式8计算的，

其中，λ₁为可调节的比例参数，且λ₁>0。

加权值α还可以根据其他方式计算，本发明并不限于此。

在一些实施例中，IMU数据包括陀螺仪数据和加速度计数据，陀螺仪数据包括当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k) w_bz(k)]^T，加速度计数据包括当前时刻t(k)的加速度矢量a(k)＝[a_bx(k) a_by(k) a_bz(k)]^T。

如图4所示，为本发明第三实施例的移动机器人的姿态估计方法的流程图，在图1中的第一实施例的基础上，上述步骤110包括：

步骤1101：确定互补滤波的初始化参数。

其中，互补滤波的初始化参数包括比例参数kp、积分参数ki和初始积分误差矢量e_Int＝[0 0 0]^T。

确定互补滤波的初始化参数是在对移动机器人进行初始化完成后，也就是说，若车体上的IMU和里程计初始化完成后，确定互补滤波的初始化参数。移动机器人的初始化操作是估计出移动机器人的初始姿态，具体详见上述实施例的说明。

步骤1102：根据公式9，计算当前时刻t(k)的积分误差矢量e_Int(k)，并进行限幅，

e_Int(k)＝e_Int(k-1)+ki*e_a*ΔT 公式9

其中，e_a表示误差矢量，且由e_a＝a′(k)×(-g′)计算得到，a′(k)为加速度矢量a(k)＝[a_bx(k) a_by(k) a_bz(k)]^T归一化后的矢量，g′为重力加速度方向上的单位矢量在载体坐标系下的投影，ΔT为IMU的更新周期。

其中，g′的计算公式如下：

其中，q₀(k-1)、q₁(k-1)、q₂(k-1)和q₃(k-1)为由时刻t(k-1)的姿态计算得到的四元数，即Q(k-1)＝[q₀(k-1) q₁(k-1) q₂(k-1) q₃(k-1)q₄(k-1)]^T，其中时刻t(k-1)的姿态包括偏航角ψ_k-1、俯仰角θ_k-1和横滚角γ_k-1。

在一些实施例中，限幅后的积分误差矢量e_Int(k)为：

其中，e_Int(k)_i为积分误差矢量e_Int(k)的第i个分量，-e_{Int_E}为最小积分误差阈值，e_{Int_E}为最大积分误差阈值。

可以看出，对积分误差矢量e_Int(k)进行限幅，先将分量e_Int(k)_i的值限制在区间[-e_{Int_E},e_{Int_E}]内，随后对进行限幅操作，从而使得分量e_Int(k)_i的值处于设置好的最小积分误差阈值-e_{Int_E}和最大积分误差阈值e_{Int_E}之间。

步骤1103：利用误差矢量e_a和限幅后的积分误差矢量e_Int(k)校正当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k) w_bz(k)]^T，得到当前时刻t(k)的校正后的角速度矢量

步骤1104：根据公式10，计算当前时刻t(k)的四元数Q(k)＝[q₀(k) q₁(k) q₂(k)q₃(k)]^T。

其中，

可以看出，

是根据当前时刻t(k)的校正后的角速度矢量

计算得到的，并且用于计算四元数Q(k)在时间间隔ΔT的变换量。

步骤1105：对当前时刻t(k)的四元数Q(k)进行处理，得到当前时刻t(k)的姿态。

该处理包括归一化和转换。先对当前时刻t(k)的四元数Q(k)进行归一化，随后将归一化的四元数Q(k)转换成姿态角(即偏航角ψ_k、俯仰角θ_k和横滚角γ_k)，从而得到当前时刻t(k)的姿态。

本实施例中，通过对积分误差量e_Int(k)进行限幅，防止长时间估计姿态时，积分误差量过大影响姿态估计的精确度，并且通过利用误差矢量e_a和限幅后的积分误差矢量e_Int(k)校正当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k) w_bz(k)]^T，提高互补滤波的估计精度。

在一些实施例中，上述步骤1103中，利用误差矢量e_a和限幅后的积分误差矢量e_Int(k)校正当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k) w_bz(k)]^T，具体如下：

先根据加速度矢量a(k)＝[a_bx(k) a_by(k) a_bz(k)]^T的模值，对比例参数kp进行自适应变化，以得到变化后的对比例参数kp′。具体地，在一些实施例中，当加速度矢量a(k)的模值大于

或小于

时，kp′小于kp；当加速度矢量a(k)的模值小于

且大于

时，kp′＝kp。g为重力加速度矢量的模值，

和

为阈值比例参数。

随后，对变化后的对比例参数kp′进行限幅，其中，若kp′<0，则kp′＝0。

再随后，根据公式11，计算得到当前时刻t(k)的校正后的角速度矢量

本实施例中，通过根据车体的运动加速度情况，对比例参数kp进行自适应变化而得到变化后的比例参数kp′，实现校正陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k) w_bz(k)]^T，得到校正后的角速度矢量

从而提高互补滤波的估计精度。

如图5所示，为本发明实施例的终端设备的结构示意图。该终端设备包括存储器510、处理器520、输出设备530以及总线540。

存储器510可以包括只读存储器和随机存取存储器，并向处理器520提供指令和数据。存储器510的一部分还可以包括非易失性存储介质，例如非易失性随机存取存储器(NVRAM)。

存储器510存储有如下的元素，可执行模块或者数据结构，或者它们的子集，或者它们的扩展集：操作指令，包括各种操作指令，用于实现各种操作；操作系统，包括各种系统程序，用于实现各种基础业务以及处理基于硬件的任务。存储器510还存储有运动轨迹，其由运动物体在视频的各视频帧中出现的位置、时间及其ID值的关联数据所表征。

输出设备530包括显示设备，例如阴极射线管(cathode ray tube，CRT)或液晶显示器(liquid crystal display，LCD)等，以及扬声器或类似的音频输出设备。一些实施例包括诸如同时用作输入设备和输出设备的触摸屏等设备。

具体的应用中，终端的各个组件通过总线540耦合在一起，其中总线540除包括数据总线之外，还可以包括电源总线、控制总线和状态信号总线等。但是为了清楚说明起见，在图中将各种总线都标为总线540。

在一些实施例中，处理器520通过调用存储器510存储的指令，可以执行如下操作：

对IMU所采集的IMU数据进行互补滤波，以估计移动机器人的运动姿态，其中运动姿态包括偏航角、俯仰角和横滚角；以及

利用里程计的更新周期和里程计所采集的里程计数据校正运动姿态中的偏航角，并将校正后的偏航角更新为运动姿态中的偏航角。

对于本发明实施例的终端设备的各个组件的功能的具体描述，请参考上述相应实施例的方法的相关描述。

上述本发明实施例揭示的方法可以应用于处理器520中，或者由处理器520实现。处理器520可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器520中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器520可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器510，处理器520读取存储器510中的信息，结合其硬件完成上述方法的步骤。

本发明还提供一种非易失性存储介质的实施例，如图6所示，该非易失性存储介质600存储有处理器可运行的指令601，该指令601用于执行上述实施例中的方法。具体地，该存储介质800具体可如图5所示的存储器510或为存储器510的一部分。

所属领域的技术人员易知，可在保持本发明的教示内容的同时对装置及方法作出诸多修改及变动。因此，以上公开内容应被视为仅受随附权利要求书的范围的限制。

Claims (10)

1.一种移动机器人的姿态估计方法，所述移动机器人包括车体，其中所述车体上固定安装有IMU和里程计，其特征在于，包括：

对所述IMU所采集的IMU数据进行互补滤波，以估计所述移动机器人的运动姿态，其中所述运动姿态包括偏航角、俯仰角和横滚角；以及

利用所述里程计的更新周期和所述里程计所采集的里程计数据校正所述运动姿态中的偏航角，并将校正后的偏航角更新为所述运动姿态中的偏航角。

2.如权利要求1中所述的方法，其特征在于，所述里程计所采集的里程计数据包括当前时刻t(k)的里程计角速度w′_bz(k)和更新时刻t(k-m)的里程计角速度w′_bz(k-m)；

所述利用所述里程计的更新周期和所述里程计所采集的里程计数据校正所述运动姿态中的偏航角包括：

获取通过互补滤波所估计的所述当前时刻t(k)的偏航角ψ_k和所述更新时刻t(k-m)的偏航角ψ_k-m，并根据公式1，计算所述更新时刻t(k-m)至所述当前时刻t(k)的第一偏航角增量

其中，所述更新周期为mΔT；

根据所述当前时刻t(k)的里程计角速度w′_bz(k)和所述更新时刻t(k-m)的里程计角速度w′_bz(k-m)，计算所述更新时刻t(k-m)至所述当前时刻t(k)的第二偏航角增量

确定所述第二偏航角增量

的加权值α，其中0≤α≤1；

根据公式2，计算最终偏航角增量

根据公式3，计算所述当前时刻t(k)的校正后的偏航角ψ’_k，

3.如权利要求2中所述的方法，其特征在于，根据所述当前时刻t(k)的里程计角速度w′_bz(k)和所述更新时刻t(k-m)的里程计角速度w′_bz(k-m)，计算所述更新时刻t(k-m)至所述当前时刻t(k)的第二偏航角增量

包括：

根据公式4，分别计算所述当前时刻t(k)的里程计角速度w′_bz(k)在导航坐标系Z轴的分量w′_nz(k)和所述更新时刻t(k-m)的里程计角速度w′_bz(k-m)在导航坐标系Z轴的分量w′_nz(k-m)，

其中，

分别为时刻t(k_i)时所述移动机器人的俯仰角和横滚角；

根据公式5，计算角速度

其中，λ₀为根据所述里程计的更新频率而变化的参数，且0≤λ₀≤1；

根据公式6，计算所述第二偏航角增量

4.如权利要求2中所述的方法，其特征在于，所述IMU数据包括陀螺仪数据和加速度计数据，所述陀螺仪数据包括所述当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k)w_bz(k)]^T；

所述确定所述第二偏航角增量

的加权值α包括：

根据公式7，估计所述移动机器人沿载体坐标系Z轴的角速度的绝对值

其中，w_bz表示陀螺仪角速度矢量w(k)在载体坐标系Z轴上分量的值，m′为可调节的参数，满足0<m′≤m；

根据里程计角速度范围的阈值ω_E确定加权值α，其中，ω_E>0，当

时，所述加权值α大于预设值，当

时，所述加权值α小于所述预设值，且所述预设值处于0到1之间。

5.如权利要求4中所述的方法，其特征在于，所述加权值α是根据公式8计算的，

其中，λ₁为可调节的比例参数，且λ₁>0。

6.如权利要求1中所述的方法，其特征在于，所述IMU数据包括陀螺仪数据和加速度计数据，所述陀螺仪数据包括所述当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k)w_bz(k)]^T，所述加速度计数据包括所述当前时刻t(k)的加速度矢量a(k)＝[a_bx(k) a_by(k)a_bz(k)]^T；

对所述IMU所采集的IMU数据进行互补滤波，以估计运动姿态，包括：

确定所述互补滤波的初始化参数，其中，所述互补滤波的初始化参数包括比例参数kp、积分参数ki和初始积分误差矢量e_Int＝[0 0 0]^T；

根据公式9，计算所述当前时刻t(k)的积分误差矢量e_Int(k)，并进行限幅，

e_Int(k)＝e_Int(k-1)+ki*e_a*ΔT 公式9，

其中，e_a表示误差矢量，且由e_a＝a′(k)×(-g′)计算得到，a′(k)为加速度矢量a(k)＝[a_bx(k) a_by(k) a_bz(k)]^T归一化后的矢量，g′为重力加速度方向上的单位矢量在载体坐标系下的投影，ΔT为所述IMU的更新周期；

利用所述误差矢量e_a和限幅后的积分误差矢量e_Int(k)校正所述当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k) w_bz(k)]^T，得到所述当前时刻t(k)的校正后的角速度矢量

根据公式10，计算所述当前时刻t(k)的四元数Q(k)＝[q₀(k) q₁(k) q₂(k) q₃(k)]^T，

其中，

Q(k-1)为时刻t(k-1)的四元数；

对所述当前时刻t(k)的四元数Q(k)＝[q₀(k) q₁(k) q₂(k) q₃(k)]^T进行处理，得到所述当前时刻t(k)的姿态。

7.如权利要求6中所述的方法，其特征在于，限幅后的积分误差矢量e_Int(k)为：

其中，e_Int(k)_i为所述积分误差矢量e_Int(k)的第i个分量，-e_{Int_E}为最小积分误差阈值，e_{Int_E}为最大积分误差阈值。

8.如权利要求6中所述的方法，其特征在于，所述利用所述误差矢量e_a和限幅后的积分误差矢量e_Int(k)校正所述当前时刻t(k)的陀螺仪角速度矢量w(k)＝[w_bx(k) w_by(k) w_bz(k)]^T，包括：

根据所述加速度矢量a(k)＝[a_bx(k) a_by(k) a_bz(k)]^T的模值，对比例参数kp进行自适应变化，以得到变化后的比例参数kp′；

对变化后的对比例参数kp′进行限幅，其中，若kp′<0，则kp′＝0；

根据公式11，计算得到所述当前时刻t(k)的校正后的角速度矢量

9.如权利要求8中所述的方法，其特征在于，当所述加速度矢量a(k)的模值大于或小于

时，kp′<kp；

当所述加速度矢量a(k)的模值小于

且大于

时，kp′＝kp；

其中，g为重力加速度矢量的模值，

和

均为阈值比例参数。

10.一种终端设备，其特征在于，包括处理器和存储器，所述存储器存储有指令，所述指令在执行时使得所述处理器执行如权利要求1-9任一项所述的移动机器人的姿态估计方法。

Images