刚体姿态追踪方法及其装置、设备、存储介质与流程

本发明涉及动作捕捉技术领域，具体涉及一种刚体姿态追踪方法及其装置、设备、存储介质。

背景技术：

刚体姿态的追踪在有标记点人体动捕/人机交互等技术的应用中是非常重要的，如果追踪技术不好，容易出现交互体验感较差、角色颤抖、容易跟丢等问题。刚体的姿态追踪方法一般有基于硬件和软件的方法，硬件的方法有激光、雷达、电磁波、超声波和imu惯性传感器等。软件的方法有基于kalman、粒子滤波、meanshift、高斯滤波、滤波等。硬件的方法虽然准确度高，但也容易受环境因素的影响，并且容易出现基线漂移、时间漂移等问题，另外，硬件的成本也比较高以及数据的融合也较难。

软件的方法中，kalman是比较常用的传统方法，它要求模型噪声和观测噪声等先验知识来进行建模，虽然它的抗高频颤抖能较强，但目标的运动和噪声存在着很大的不确定性，使得它容易存在延迟、目标运动方向剧烈变化的时候容易存在误差较大等问题；高斯滤波存在着时间窗口的约束，容易出现过滤波等问题；滤波虽然响应较快，可以较好的追踪动态信号，延迟较低，但对震颤的抗性较差。因此，有必要解决这一问题。

技术实现要素：

本发明主要解决的技术问题是如何解决kalman容易存在延迟、目标运动方向剧烈变化的时候容易存在误差较大的情形以及滤波震颤的抗性较差的问题。为克服上述技术问题，本申请公开一种刚体姿态追踪方法及其装置、设备、存储介质。

本发明结合kalman原理和滤波原理两种算法在现有的刚体姿态解算方法基础上进行刚体姿态的追踪计算，具体方法如下：

根据第一方面，一种实施例中提供一种刚体姿态追踪方法，包括：根据预设的函数模型分别建立卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp，其中，所述函数模型包括速度和加速度状态变量；

获取刚体初始帧的姿态四元数和位移，得到初始化后的模型fkq、fkp、fεq、fεp；

根据所述初始化后的模型fkq、fkp、fεq、fεp分别预测刚体当前帧的姿态四元数和位移，得到姿态四元数预测值和位移预测值；

将所述姿态四元数预测值和位移预测值以及获取的刚体当前帧姿态四元数和位移真实值引入二范数函数中，计算得到刚体当前帧的姿态信息。

所述根据预设的函数模型分别建立卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp，其中，所述函数模型包括速度和加速度状态变量，包括：

将速度和加速度加入卡尔曼滤波的状态维度因子，结合滤波器原理，得到所述预设的函数模型；

根据所述预设的函数模型分别建立卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp。

所述将速度和加速度加入卡尔曼滤波的状态维度因子包括：

选用kalman滤波模型，得到输出刚体姿态四元数和位移预测值的kalman滤波公式：

其中v为速度，a为加速度，q为协方差矩阵，f为状态转移矩阵。

所述滤波器原理包括：

令刚体姿态四元数和位移的预测值在ti时刻为xi，滤波后的预测值为则：

其中，α∈[0，1]代表平滑因子。

所述卡尔曼预测四元数模型fkq和卡尔曼预测位移模型fkp均包括：状态参数维度和测量变量，所述fkq的状态参数维度、测量变量个数分别为12、4，所述fkp的状态参数维度、测量变量个数分别为9、3。

所述将所述姿态四元数预测值和位移预测值以及获取的刚体当前帧姿态四元数和位移真实值引入二范数函数中，计算得到刚体当前帧的姿态信息包括：

将所述姿态四元数预测值qkframe、qεframe和位移预测值pkframe、pεframe，以及获取的刚体当前帧姿态四元数真实值qtframe和位移真实值ptframe引入二范数函数中，以进行比较，求得刚体当前帧的姿态四元数qframe和位移pframe，所述二范数函数为：

distq＝||qkfame-qtframe||

distp＝||pkframe，-ptframe||

其中thq、thp分别为一固定阈值。

所述thq、thp根据调试结果得出，其取值范围为0-0.01。

根据第二方面，一种实施例中提供一种刚体姿态追踪装置，其特征在于，包括：构建单元，用于根据预设的函数模型分别建立卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp，其中，所述函数模型包括速度和加速度状态变量；

预测单元，用于获取刚体初始帧的姿态四元数和位移，得到初始化后的所述模型fkq、fkp、fεq、fεp；根据所述模型fkq、fkp、fεq、fεp分别预测刚体当前帧的姿态四元数和位移，得到姿态四元数预测值和位移预测值；

计算单元，用于将所述姿态四元数预测值和位移预测值以及获取的刚体当前帧姿态四元数和位移真实值引入二范数函数中，计算得到刚体当前帧的姿态信息。

根据第三方面，一种实施例中提供一种设备，包括：存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的刚体姿态追踪方法程序，所述刚体姿态追踪方法程序被所述处理器执行时实现如权利要求1-7中任一项所述的刚体姿态追踪方法的步骤。

根据第四方面，一种实施例中提供一种计算机可读存储介质，包括程序，所述程序能够被处理器执行以实现上述第一方面所述的方法。

本申请的有益效果是：

依据上述实施例的一种刚体姿态追踪方法，该方法包括：根据预设的函数模型分别建立卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp，函数模型加入了速度和加速度状态变量；获取刚体初始帧的姿态四元数和位移，得到初始化后的模型fkq、fkp、fεq、fεp；根据初始化后的模型fkq、fkp、fεq、fεp分别预测刚体当前帧的姿态四元数和位移，得到姿态四元数预测值和位移预测值；将姿态四元数预测值和位移预测值以及获取的刚体当前帧姿态四元数和位移真实值引入二范数函数中，计算得到刚体当前帧的姿态信息。通过该方法，引入速度和加速度概念，结合kalman原理和虑波原理两种算法，将得到的预测值与真实值进行对比，从而达到追踪刚体姿态信息的目的，因此，比较明显地减缓了kalman容易存在延迟、目标运动方向剧烈变化的时候容易存在误差较大的问题，以及滤波震颤的抗性较差的问题，使得系统延迟较低，刚体追踪的抗震颤效果显著提升。由于这两种算法的计算速度均非常快，因此刚体追踪的实时性也较高。

附图说明

图1为本申请中刚体姿态追踪方法的流程图；

图2为本申请中刚体姿态追踪装置的结构示意图；

图3为本申请中实现刚体姿态追踪方法程序的设备的结构示意图。

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。其中不同实施方式中类似元件采用了相关联的类似的元件标号。在以下的实施方式中，很多细节描述是为了使得本申请能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其他元件、材料、方法所替代。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。

另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。

本文中为部件所编序号本身，例如“第一”、“第二”等，仅用于区分所描述的对象，不具有任何顺序或技术含义。而本申请所说“连接”、“联接”，如无特别说明，均包括直接和间接连接(联接)。

实施例一、

请参考图1，本申请公开一种刚体姿态追踪方法，该方法包括步骤s110-s140，大致思路为：第一步在原来kalman滤波模型基础上加入速度和加速度，结合滤波模型获取刚体姿态四元数和位移的预测值；第二步根据第一步单个的kalman滤波模型和滤波模型分别建立卡尔曼预测四元数模型、卡尔曼预测位移模型、滤波预测四元数模型、滤波预测位移模型；第三步，利用现有的刚体姿态解算技术获取刚体初始帧的姿态四元数和位移，并根据初始帧的姿态四元数和位移值初始化上述四个模型；第四步，利用初始化后的四个模型公式计算出预测的四元数和位移值，然后结合当前真实姿态四元数和位移，根据二范数模型公式计算出目标刚体当前的姿态信息，下面将分别说明。

步骤s110，根据预设的函数模型分别建立卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp，其中，函数模型包括速度和加速度状态变量；

在一个实施例中，首先根据已知的kalman滤波模型和滤波模型，考虑引入速度v和加速度a，使其加入卡尔曼滤波的状态维度因子，得到选用的kalman(卡尔曼)滤波模型为：

vt＝vt-1+δta

其中q为协方差矩阵，f为状态转移矩阵，利用该滤波模型可更新协方差矩阵和状态矩阵，从而获取刚体姿态四元数和位移的预测值。

上述加入速度和加速度后的kalman滤波模型结合滤波器原理，便可得到预设的函数模型，根据预设的函数模型，就可以建立四个用来求取预测值的卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp。

相应地，根据滤波器原理得到刚体姿态四元数和位移预测值的预测过程具体为：设刚体姿态四元数和位移的预测值在ti时刻未滤波的值为xi，滤波后的值为那么有：

其中，平滑因子α∈[0，1]不是一个常数，而是自适应的，也就是说，利用信号的变化率(速度)信息动态计算。自适应平滑因子的目的是平衡抖动和延迟权衡，因为在低速时对抖动很敏感，而在高速时对延迟更敏感。平滑因子定义为：

其中τ为时间常量，te为采样间隔时间，滤波后的值便是需要求得的刚体姿态四元数和位移的预测值。

步骤s120，获取刚体初始帧的姿态四元数和位移，得到初始化后的模型fkq、fkp、fεq、fεp；

利用现有的刚体姿态解算技术获取刚体第0帧即初始帧的姿态四元数q0和位移p0，将四元数q0和位移p0值输入上述建立的四个模型中，分别得到初始化后的用于追踪姿态四元数的卡尔曼预测四元数模型fkq(q0，q1，...，qi-1)和用于追踪姿态位移的卡尔曼预测位移模型fkp(q0，q1，...，qi-1)以及用于追踪姿态四元数的滤波预测四元数模型fεq(q0，q1，...，qi1)和用于追踪姿态位移的滤波预测位移模型fεp(q0，q1，...，qi-1)，其中i≥1。现有的刚体姿态解算技术为已知技术，此处不再赘述。

考虑到刚体运动的不确定性，就需要在卡尔曼滤波模型中加入速度和加速度状态变量，此时，卡尔曼预测四元数模型fkq的状态参数维度、测量变量个数分别变为12、4，卡尔曼预测位移模型fkp的状态参数维度、测量变量个数分别变为9、3，其它模型参数根据系统的性能设置。

步骤s130，根据初始化后的模型fkq、fkp、fεq、fεp分别预测刚体当前帧的姿态四元数和位移，得到姿态四元数预测值和位移预测值；

利用初始化后的四个模型fkq、fkp、fεq、fεp分别预测第frame(frame≥1)帧的姿态四元数和位移，即分别预测刚体当前帧的姿态四元数和位移，得到姿态四元数预测值qkframe、qεframe和位移预测值pkframe、pεframe。

步骤s140，将姿态四元数预测值和位移预测值以及获取的刚体当前帧姿态四元数和位移真实值引入二范数函数中，计算得到刚体当前帧的姿态信息。

在本实施例中，计算当前刚体姿态四元数和位移的真实值后，获取得到刚体当前帧姿态四元数真实值qtframe和位移真实值ptframe，并将姿态四元数预测值qkframe、qεframe和位移预测值pkframe、pεframe引入二范数函数中，根据以下二范数函数模型计算目标刚体当前的姿态信息qframe、pframe：

distq＝||qkframe-qtframe||

distp＝||pkframe-ptframe||

其中thq、thp分别为一固定阈值，取值大约为0.01左右，一般取值范围为：0-0.01，都是根据调试结果得出。

需要理解的是，本申请刚体追踪的含义就是利用刚体在某时间t0到ti-1的姿态先验信息，来预测ti时刻的姿态信息，便于与系统求取的ti时刻的刚体真实姿态信息进行比较，即对姿态四元数与位移求二范数，当姿态四元数比较结果小于固定阈值thq时，求得刚体当前的姿态四元数qframe为qkframe，反之求得刚体当前的姿态四元数qframe为qεframe，当位移比较结果小于固定阈值thp时，求得刚体当前位移pframe为pkframe，反之，求得刚体当前位移pframe为pεframe，通过此方法，刚体一旦失踪，则利用预测的刚体姿态信息进行反向求解，从而达到刚体追踪的目的。

实施例二、

请参考图2，本申请公开一种刚体姿态追踪装置3，该装置主要包括构建单元31、预测单元32和计算单元33，下面分别说明。

构建单元31，用于根据预设的函数模型分别建立卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp，其中，函数模型包括速度和加速度状态变量；

预测单元32，用于获取刚体初始帧的姿态四元数和位移，得到初始化后的模型fkq、fkp、fεq、fεp；根据初始化后的模型fkq、fkp、fεq、fεp分别预测刚体当前帧的姿态四元数和位移，得到姿态四元数预测值和位移预测值；可见图2，获取刚体初始帧的姿态四元数和位移通过动作捕捉装置追踪刚体的运动情况获得；

计算单元33，用于将姿态四元数预测值和位移预测值以及获取的刚体当前帧姿态四元数和位移真实值引入二范数函数中，计算得到刚体当前帧的姿态信息，其中，获取的刚体当前帧姿态四元数和位移真实值也通过动作捕捉装置追踪刚体的运动情况获得。关于计算单元33的具体功能，可以参考步骤s140中的相关内容，这里不再进行详细说明。

实施例三、

请参考图3，本申请还公开一种设备4，该设备4可以包括存储器41和处理器42，以及包括存储在存储器41上并可在处理器42上运行的刚体姿态追踪方法程序。此外需要说明的是，这里的刚体姿态追踪方法程序能够在被处理器42执行时实现实施例一中关于刚体姿态追踪方法的步骤(即步骤s110-s140)。

需要说明的是，在本实施例中，存储器41上存储的刚体姿态追踪方法程序可以包括一些功能模块，如实施例三中请求保护的构建单元31、预测单元32和计算单元33。其中，构建单元31用于根据预设的函数模型分别建立卡尔曼预测四元数模型fkq、卡尔曼预测位移模型fkp、滤波预测四元数模型fεq、滤波预测位移模型fεp；预测单元32与构建单元31连接，用于根据建立的模型对其进行初始化，并根据初始化后的模型预测刚体当前帧的姿态四元数和位移；计算单元33与预测单元32连接，用于将已经获得的姿态四元数预测值和位移预测值以及刚体当前帧姿态四元数和位移真实值引入二范数函数中，以完成对刚体姿态进行追踪的目的。

本申请利用刚体在某时刻的姿态先验信息，来预测当前时刻的姿态信息，便于与刚体真实姿态信息进行比较，通过此方法，刚体一旦失踪，则还可以利用预测的刚体姿态信息进行反向求解，从而达到追踪刚体姿态信息的目的，因此，本申请采用结合kalman原理和滤波原理的方法，比较明显地减缓了kalman容易存在延迟、目标运动方向剧烈变化的时候容易存在误差较大的问题，以及滤波震颤的抗性较差的问题，使得系统延迟较低，刚体追踪的抗震颤效果显著提升。由于这两种算法的计算速度均非常快，因此刚体追踪的实时性也较高。

本领域技术人员可以理解，上述实施方式中各种方法的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的系统进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。