CN113864387A - 一种主动减震机构控制方法、系统和存储介质 - Google Patents

Abstract

本发明公开了一种主动减震机构控制方法、系统和存储介质，其方法包括步骤：获取定平台的第一实时位姿与第一实时速度；根据预设的动平台的第一期望位姿、第一期望速度，得到两平台的位姿差和速度差；根据连接结构的运动学模型，计算动平台平衡时连接结构中连接轴的第二期望位置与第二期望速度；获取连接轴的第二实时速度和第二实时位置、每根连接轴与驱动电机的传动关系，以及连接结构的动力学模型，计算每根连接轴的力矩前馈；根据连接轴的第二期望位置、第二期望速度和力矩前馈控制连接轴对应的驱动电机。本发明提高主动减震机构的减震效果。

Description

一种主动减震机构控制方法、系统和存储介质

技术领域

本发明涉及减震技术领域，特别涉及一种主动减震机构控制方法、系统和存储介质。

背景技术

在移动载具的工作场景中，经常出现驾驶过程过于颠簸影响移动载具的自平衡控制，例如行车过程中道路颠簸以及在舰船行驶过程中抵抗海浪颠簸的场景中，现有的减震效果不理想影响移动载具的平衡性。在现有的减振设备中，大多数是采用被动减振的方式来减振，例如液压装置或者弹簧装置。但是这些方法响应速度慢，在高频振动下，减振效果并不明显。而主动减振的方式由于技术不成熟，也无法提供较好的减震效果。

目前常用的主动减振方法是基于加速度补偿的方式，但是这种方式不会考虑在主动减震机构的姿态发生变化时产生的影响，会使得补偿的效果并不理想；

而且市面上的传感器所能测得的速度、加速度数据存在一定程度上的漂移与误差，直接使用这些数据来补偿会使减震机构位移迅速发散；同时在高频振动下，主动减振机构的驱动装置的反应速度难以跟随颠簸环境下的路面激励，其位姿平衡存在滞后性，难以达成理想的减振效果；并且在主动减振机构的控制中往往需要并联机器人的正解来对驱动装置进行控制，而很多构型下的多轴并联机器人很难获得其运动学正解。

针对上述由于主动减震技术不成熟，导致无法提供较好的减震效果的技术问题，本发明提供一种主动减震机构控制方法，提高主动减震效果。

发明内容

为解决主动减震技术不成熟，导致无法提供较好的减震效果的技术问题，本发明提供一种主动减震机构控制方法、系统和存储介质，具体的技术方案如下：

本发明提供一种主动减震机构控制方法，应用于控制多自由度的主动减振平台，所述主动减震平台包括动平台、定平台，以及连接所述动平台与所述定平台的连接结构，所述连接结构包括若干根连接轴，每根所述连接轴均对应一驱动电机，包括步骤：

获取所述定平台的第一实时位姿与第一实时速度；

分别根据预设的所述动平台的第一期望位姿、第一期望速度、所述定平台的所述第一实时位姿和所述第一实时速度，得到所述动平台和所述定平台的位姿差和速度差；

根据所述位姿差、所述速度差，以及所述连接结构的运动学模型，计算所述动平台平衡时所述连接结构中每根所述连接轴的第二期望位置与第二期望速度；

获取所述连接结构中每根所述连接轴的第二实时速度和第二实时位置、每根所述连接轴与所述驱动电机的传动关系，以及所述连接结构的动力学模型，并计算得到每根所述连接轴的力矩前馈；

根据所述连接结构中每根所述连接轴的所述第二期望位置、所述第二期望速度和所述力矩前馈控制每根所述连接轴对应的所述驱动电机。

本发明提供的主动减震机构控制方法通过动平台与定平台之间的位姿差和速度差，结合主动减震机构的运动学模型和动力学模型，计算动平台与定平台之间每根连接轴的期望位姿、期望速度以及力矩前馈，根据期望位姿、期望速度以及力矩前馈控制每个连接轴的驱动电机，使主动减震平台处于平衡状态，减少外力干扰对于主动减震平台稳定性的影响。

进一步地，本发明还提供一种主动减震机构控制方法，所述的计算得到所述定平台的第一实时位姿与第一实时速度，具体包括：

对所述实时加速度进行低通滤波处理，对所述实时角速度进行高通滤波处理；

根据所述定平台的随体坐标系与惯性坐标系之间的旋转矩阵，得到所述定平台初始时刻的初始俯仰角、初始颠簸角和初始扭转角；

根据所述初始俯仰角、所述初始颠簸角、所述初始扭转角以及所述实时角速度和所述实时加速度，得到所述定平台的实时俯仰角、实时颠簸角和实时扭转角；

通过所述实时俯仰角、所述实时颠簸角和所述实时扭转角计算所述第一实时位姿和所述第一实时速度。

本发明提供的主动减震机构控制方法利用互补滤波，分别对测得的加速度使用低通滤波处理，对测得的角速度使用高通滤波处理，根据滤波后准确的实时加速度和实时角速度通过计算实时俯仰角、实时颠簸角和实时扭转角，得到定平台的实时位姿与实时速度，避免面前大多数基于加速度补偿的方式中平台姿态发生变化产生的影响，提高减震效果。

进一步地，本发明还提供一种主动减震机构控制方法：

所述主动减震机构在俯仰角、扭转角和颠簸角三个自由度上包括若干个耦合自由度和若干个独立自由度；

所述的对所述实时加速度进行低通滤波处理，对所述实时角速度进行高通滤波处理之后，还包括：

根据所述定平台的随体坐标系与惯性坐标系之间的旋转矩阵，得到所述定平台初始时刻若干个所述独立自由度对应的初始角度；

根据若干个所述初始角度以及所述实时角速度和所述实时加速度，得到所述定平台若干个所述独立自由度对应的实时角度；

根据若干个所述独立自由度对应的实时角度得到若干个所述耦合自由度对应的实时角度；

通过若干个所述独立自由度对应的实时角度和若干个所述耦合自由度对应的实时角度计算所述第一实时位姿和所述第一实时速度。

本发明提供的主动减震机构控制方法在俯仰角、扭转角和颠簸角三个自由度上存在耦合自由度时，通过获取独立自由度的角度计算耦合自由度的角度，并计算定平台的实时位姿与实时速度，适用于多种结构的主动减震机构。

进一步地，本发明还提供一种主动减震机构控制方法，所述的计算得到所述定平台的第一实时位姿与第一实时速度之后，所述的得到所述动平台和所述定平台的位姿差和速度差之前，还包括：

选取所述动平台在俯仰角、扭转角和颠簸角以及横向、竖向和纵向六个自由度中的若干个减振自由度；

设置所述第一期望位姿在若干个所述减振自由度上所述第一期望位姿值和所述第一期望速度值为0，在若干个其余自由度上所述第一期望位姿值和所述第一期望速度值，与所述定平台在对应的自由度上所述实时位姿值和所述实时速度值相同，得到所述第一期望位姿和所述第一期望速度。

本发明提供的主动减震机构控制方法提供一种根据主动设置的自由度，预设的动平台的期望位姿和期望速度的方法，使本发明可以根据主动选择的自由度，在该自由度上进行减震。

进一步地，本发明还提供一种主动减震机构控制方法，所述的根据所述位姿差、所述速度差，以及所述连接结构的运动学模型，计算所述动平台平衡时所述连接结构中每根所述连接轴的第二期望位置与第二期望速度，具体包括：

根据所述主动减震机构的构型，以及所述主动减震机构的运动学逆解，得到每根所述连接轴的长度与所述位姿差的换算比例作为第一运动学模型；

对所述第一运动学模型中每根所述连接轴的长度求导，得到各个所述驱动电机的速度、所述位姿差和所述速度差的对应关系作为第二运动学模型；

根据所述第一运动学模型、所述第二运动模型、所述位姿差和所述速度差得到各个所述驱动电机的速度作为对应的每根所述连接轴的所述第二期望速度，每根所述连接轴的长度作为所述第二期望位置。

本发明提供的主动减震机构控制方法通过主动减震机构的运动学逆解和主动减震机构的构型得到第一运动学模型和第二运动学模型，并根据位姿差和速度差结合两个运动学模型，计算每根连接轴的期望位置和期望速度。通过运动学逆解进行姿态控制，由于逆解算法简单且有精准的解析解的形式，可以避免主动减震机构的运动学的正解会因为其构型的不同，无法得出精确的解析解的情况。采用本方法提高每根连接轴的期望位置和期望速度的计算效率，保证主动减震平台对外界环境变化实时响应。

进一步地，本发明还提供一种主动减震机构控制方法，所述的获取所述连接结构中每根所述连接轴的第二实时速度和第二实时位置、每根所述连接轴与所述驱动电机的传动关系，以及所述连接结构的动力学模型，计算得到每根所述连接轴的力矩前馈，具体包括：

获取所述连接结构中每根所述连接轴的实时出力与所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项的关系作为所述动力学模型；

获取所述关节空间内每根所述连接轴的所述实际位置和轴向上的所述实际速度，并计算得到每根所述连接轴轴向上的实际加速度；

根据所述连接结构中每根所述连接轴的所述实际速度、所述实际位置和所述实际加速度，计算所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项；

根据所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项和所述动力学模型，计算所述连接结构中每根所述连接轴的所述实时出力；

获取每根所述连接轴与所述驱动电机的传动比，并根据所述传动比和每根所述连接轴的所述实际出力计算每根所述连接轴的所述力矩前馈。

本发明提供的主动减震机构控制方法通过主动减震机构的动力学模型、每根连接轴的实际位置和轴向上的实际速度，通过计算每根连接轴的实时出力作为中间量，得到连接轴的力矩前馈。并通过前馈力矩的方式优化主动减震平台在应对高频振动时的表现，使驱动部分的反应速度可以跟随高频激励，提高整体减振效果。

进一步地，本发明提供的一种主动减震机构控制方法，所述的根据所述连接结构中每根所述连接轴的所述第二期望位置、所述第二期望速度和所述力矩前馈控制每根所述连接轴对应的所述驱动电机，具体包括：

将所述第二期望位置、所述第二期望速度和所述力矩前馈导入各个所述驱动电机的控制框架中，生成各个所述驱动电机对应的PID参数；

根据各个所述PID参数控制对应的所述驱动电机。

本发明提供的主动减震机构控制方法将每根连接轴的期望位置、期望速度和力矩前馈导入驱动电机的控制框架中，实时更新各个驱动电机对应的PID参数，通过驱动电机控制每根轴的运动，使主动减震平台处于稳定状态，提高减震效果。

另外地，本发明还提供一种主动减震机构控制系统，应用于控制多自由度的主动减振平台，所述主动减震平台包括动平台、定平台，以及连接所述动平台与所述定平台的连接结构，所述连接结构包括若干根连接轴，每根所述连接轴均对应一驱动电机，包括：

第一获取模块，用于获取所述定平台的实时位姿与实时速度；

第一计算模块，与所述第一获取模块连接，用于分别根据预设的所述动平台的第一期望位姿、第一期望速度、所述定平台的所述第一实时位姿和所述第一实时速度，得到所述动平台和所述定平台的位姿差和速度差；

第二计算模块，与所述第一计算模块连接，用于根据所述位姿差、所述速度差，以及所述连接结构的运动学模型，计算所述动平台平衡时所述连接结构中每根所述连接轴的第二期望位置与第二期望速度；

第三计算模块，用于获取所述连接结构中每根所述连接轴的第二实时速度和第二实时位置、每根所述连接轴与所述驱动电机的传动关系，以及所述连接结构的动力学模型，并计算得到每根所述连接轴的力矩前馈；

控制模块，与所述第二计算模块和所述第三计算模块连接，用于根据所述连接结构中每根所述连接轴的所述第二期望位置、所述第二期望速度和所述力矩前馈控制每根所述连接轴对应的所述驱动电机。

进一步地，本发明提供的一种主动减震机构控制系统，所述第三计算模块包括：

动力学模型获取单元，用于获取所述连接结构中每根所述连接轴的实时出力与所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项的关系作为所述动力学模型；

第一计算单元，用于获取所述关节空间内每根所述连接轴的所述实际位置和轴向上的所述实际速度，并计算得到每根所述连接轴轴向上的实际加速度；

第二计算单元，与所述第一计算单元连接，用于根据所述连接结构中每根所述连接轴的所述实际速度、所述实际位置和所述实际加速度，计算所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项；

第三计算单元，与所述动力学模型获取单元和所述第二计算单元连接，用于根据所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项和所述动力学模型，计算所述连接结构中每根所述连接轴的所述实时出力；

第四计算单元，与所述第三计算单元连接，用于获取每根所述连接轴与所述驱动电机的传动比，并根据所述传动比和每根所述连接轴的所述实际出力计算每根所述连接轴的所述力矩前馈。

另外地，本发明提供的一种存储介质，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现如权利要求1～7中任意一项所述的主动减震机构控制方法所执行的操作。

本发明提供一种主动减震机构控制方法、系统和存储介质，至少包括以下一项技术效果：

(1)通过动平台与定平台之间的位姿差和速度差，结合主动减震机构的运动学模型和动力学模型，计算动平台与定平台之间每根连接轴的期望位姿、期望速度以及力矩前馈，根据期望位姿、期望速度以及力矩前馈控制每个连接轴的驱动电机，使主动减震平台处于平衡状态，减少外力干扰对于主动减震平台稳定性的影响；

(2)利用互补滤波，分别对测得的加速度使用低通滤波处理，对测得的角速度使用高通滤波处理，根据滤波后准确的实时加速度和实时角速度通过计算实时俯仰角、实时颠簸角和实时扭转角，得到定平台的实时位姿与实时速度，避免面前大多数基于加速度补偿的方式中平台姿态发生变化产生的影响，提高减震效果；

(3)在俯仰角、扭转角和颠簸角三个自由度上存在耦合自由度时，通过获取独立自由度的角度计算耦合自由度的角度，并计算定平台的实时位姿与实时速度，适用于多种结构的主动减震机构；

(4)根据主动设置的自由度，预设的动平台的期望位姿和期望速度，可以根据主动选择的自由度，在该自由度上进行减震；

(5)通过运动学逆解进行姿态控制，由于逆解算法简单且有精准的解析解的形式，可以避免主动减震机构的运动学的正解会因为其构型的不同，无法得出精确的解析解的情况。采用本方法提高每根连接轴的期望位置和期望速度的计算效率，保证主动减震平台对外界环境变化实时响应；

(6)通过前馈力矩的方式优化主动减震平台在应对高频振动时的表现，使驱动部分的反应速度可以跟随高频激励，提高整体减振效果。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一种基于主动减震机构控制方法的流程图；

图2为本发明一种主动减震机构控制方法中计算第一实时位姿和第一实时速度的流程图；

图3为本发明一种主动减震机构的结构示例图；

图4为本发明一种主动减震机构控制方法中计算第一实时位姿和第一实时速度的另一个流程图；

图5为本发明一种主动减震机构控制方法的一个流程图；

图6为本发明一种主动减震机构控制方法的另一个流程图；

图7为本发明一种主动减震机构控制系统的示例图；

图8为本发明一种主动减震机构控制系统的另一个示例图。

图中标号：第一获取模块10、第一计算模块20、第二计算模块30、第三计算模块40、动力学模型获取单元41、第一计算单元42、第二计算单元43、第三计算单元44、第四计算单元45、控制模块50。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其他实施例中也可以实现本申请。在其他情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所述描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其他特征、整体、步骤、操作、元素、组件和/或集合的存在或添加。

为使图面简洁，各图中只示意性地表示出了与本发明相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘出了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。

还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

另外，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。

实施例1

本发明的一个实施例，如图1、图3所示，本发明提供一种主动减震机构控制方法，其特征在于，应用于控制多自由度的主动减振平台，主动减震平台包括动平台、定平台，以及连接动平台与定平台的连接结构，连接结构包括若干根连接轴，每根连接轴均对应一驱动电机，包括步骤：

S100获取定平台的第一实时位姿与第一实时速度。

具体地，获取定平台在惯性坐标系下俯仰角、扭转角和颠簸角的实时角速度以及横向、竖向和纵向的实时加速度，计算得到定平台的第一实时位姿与第一实时速度。

主动减振平台的动平台相对于定平台具有多个自由度，常见的自由度为俯仰角、扭转角和颠簸角以及横向、竖向和纵向六个自由度，动平台相对于定平台也可以在其他方向上存在自由度，在其他方向上的自由度与这六个方向上的自由度控制方法相同。

其中动平台与定平台通过至少两个独立的运动链相连接，机构具有两个及以上的自由度，且以并联方式驱动的一种闭环机构。如图3所示的主动减振平台采用了四个独立的运动链相连接，两处下平台与连接下平台的机构以虎克铰的方式连接，另两处下平台与连接下平台的机构以转动副的方式连接。同时，上平台与连接上平台的机构全部以球铰的方式连接。我们可以控制动平台相对于下平台在所有六个自由度上运动。

由于主动减震平台是根据实时运动受力情况进行实时的减震调整，因此需要实时获取实时加速度和实时角速度，便于计算实时位姿和实时速度。

S200分别根据预设的动平台的第一期望位姿、第一期望速度、定平台的第一实时位姿和第一实时速度，得到动平台和定平台的位姿差和速度差。

具体地，根据实际期望的减震效果，预设动平台的第一期望位姿和第一期望速度，并将第一期望位姿和第一期望速度与定平台实时检测到的第一实时位姿和第一实时速度作差，得到动平台和定平台的位姿差和速度差。

S300根据位姿差、速度差，以及连接结构的运动学模型，计算动平台平衡时连接结构中每根连接轴的第二期望位置与第二期望速度。

具体地，预先根据主动减震平台的构型获取其连接结构的运动学模型。建立位姿差、速度差和连接结构中每根连接轴的第二期望位置与第二期望速度的对应关系。

将计算得到的动平台和定平台的位姿差和速度差代入运动学模型中，得到第二期望位置与第二期望速度。

S400获取连接结构中每根连接轴的第二实时速度和第二实时位置、每根连接轴与驱动电机的传动关系，以及连接结构的动力学模型，并计算得到每根连接轴的力矩前馈。

具体地，由于每根连接轴均与对应的驱动电机连接，因此通过每根连接轴对应的驱动电机可以获取第二实时速度和第二实时位置。

根据主动减震平台的结构，获取其动力学模型，将第二实时速度和第二实时位置代入动力学模型中，代入动力学模型中可以得到每根轴的实际出力，并通过每根轴与驱动电机的传动比，计算得到每根连接轴的力矩前馈。

S500根据连接结构中每根连接轴的第二期望位置、第二期望速度和力矩前馈控制每根连接轴对应的驱动电机。

本实施例提供的主动减震机构控制方法通过动平台与定平台之间的位姿差和速度差，结合主动减震机构的运动学模型和动力学模型，计算动平台与定平台之间每根连接轴的期望位姿、期望速度以及力矩前馈，根据期望位姿、期望速度以及力矩前馈控制每个连接轴的驱动电机，使主动减震平台处于平衡状态，减少外力干扰对于主动减震平台稳定性的影响。

实施例2

基于实施例1，如图2～4所示，本发明提供的一种主动减震机构控制方法，其中步骤S100获取定平台的第一实时位姿与第一实时速度，具体包括：

S110获取定平台在惯性坐标系下俯仰角、扭转角和颠簸角的实时角速度以及横向、竖向和纵向的实时加速度。

示例性地，通过传感器或惯性传导单元实时的获取定平台在惯性坐标系下三个角速度ω_mα、ω_mβ、ω_mγ与三个加速度α_nx、α_ny、α_nz。

S120对实时加速度进行低通滤波处理，对实时角速度进行高通滤波处理。

具体地，因为传感器或惯性传导单元所获取的值会有漂移误差，所以利用互补滤波，分别对传感器测得的加速度使用低通滤波处理，对测得的角速度使用高通滤波处理。

S131根据定平台的随体坐标系与惯性坐标系之间的旋转矩阵，得到定平台初始时刻的初始俯仰角、初始颠簸角和初始扭转角。

示例性地，如图3所示，在图3的主动减震机构中初始俯仰角α_m0＝arctan(α_ny/α_nc)，初始扭转角β_m0为0，初始颠簸角γ_m0＝-arcsin(α_nx/g)。

S141根据初始俯仰角、初始颠簸角、初始扭转角以及实时角速度和实时加速度，得到定平台的实时俯仰角、实时颠簸角和实时扭转角。

示例性地，实时俯仰角α_mB＝α_m0+ω_αt，实时扭转角β_mB为0，实时颠簸角γ_mB＝γ_m0+ω_γt。

S151通过实时俯仰角、实时颠簸角和实时扭转角计算第一实时位姿和第一实时速度。

具体地，通过上述计算得到的加速度和时间，计算实时动平台的第一实时位姿和第一实时速度。

可选地，如图4所示，在主动减震机构在俯仰角、扭转角和颠簸角三个自由度上包括若干个耦合自由度和若干个独立自由度时，S120对实时加速度进行低通滤波处理，对实时角速度进行高通滤波处理之后，还包括：

S132根据定平台的随体坐标系与惯性坐标系之间的旋转矩阵，得到定平台初始时刻若干个独立自由度对应的初始角度。

示例性地，主动减震机构在俯仰角和颠簸角两个自由度上为独立自由度，在扭转角自由度上为耦合自由度。

S142根据若干个初始角度以及实时角速度和实时加速度，得到定平台若干个独立自由度对应的实时角度。

S152根据若干个独立自由度对应的实时角度得到若干个耦合自由度对应的实时角度。

具体地，通过主动减震机构的构型，以及主动减震机构的运动学，由主动减震机构在俯仰角和颠簸角这两个独立自由度上的实时角度，计算得到再颠簸角这个耦合自由度上的实时角度。

S162通过若干个独立自由度对应的实时角度和若干个耦合自由度对应的实时角度计算第一实时位姿和第一实时速度。

本实施例提供的主动减震机构控制方法利用互补滤波，分别对测得的加速度使用低通滤波处理，对测得的角速度使用高通滤波处理，根据滤波后准确的实时加速度和实时角速度通过计算实时俯仰角、实时颠簸角和实时扭转角，得到定平台的实时位姿与实时速度，避免面前大多数基于加速度补偿的方式中平台姿态发生变化产生的影响，提高减震效果。并且在俯仰角、扭转角和颠簸角三个自由度上存在耦合自由度时，通过获取独立自由度的角度计算耦合自由度的角度，并计算定平台的实时位姿与实时速度，适用于多种结构的主动减震机构。

实施例3

基于实施例1～2中任意一个实施例，如图5所示，本发明提供一种主动减震机构控制方法，其中步骤S200分别根据预设的动平台的第一期望位姿、第一期望速度、定平台的第一实时位姿和第一实时速度，得到动平台和定平台的位姿差和速度差，具体包括：

S210获取预设的动平台的第一期望位姿、第一期望速度、定平台的第一实时位姿和第一实时速度。

S220选取动平台在俯仰角、扭转角和颠簸角以及横向、竖向和纵向六个自由度中的若干个减振自由度。

S230设置第一期望位姿在若干个减振自由度上第一期望位姿值和第一期望速度值为0，在若干个其余自由度上第一期望位姿值和第一期望速度值，与定平台在对应的自由度上实时位姿值和实时速度值相同，得到第一期望位姿和第一期望速度。

具体地，第一期望位姿为(x_A、y_A、z_A、α_A、β_A、γ_A),第一期望速度为

在六个自由度中选取需要减震的自由度，设置其第一期望位姿值和第一期望速度值为0，其余第一期望位姿值和第一期望速度值与定平台对应相等。

示例性地，选取X和α自由度上进行减震，第一期望位姿和第一期望速度中对应的x_A和

为0，其余值与定平台对应相等。

S240得到动平台和定平台的位姿差和速度差。

具体地，将第一期望位姿和第一期望速度与定平台实时检测到的第一实时位姿和第一实时速度作差，得到动平台和定平台的位姿差和速度差。

步骤S300根据位姿差、速度差，以及连接结构的运动学模型，计算动平台平衡时连接结构中每根连接轴的第二期望位置与第二期望速度，具体包括：

S310根据主动减震机构的构型，以及主动减震机构的运动学逆解，得到每根连接轴的长度与位姿差的换算比例作为第一运动学模型。

示例性地，第一运动学模型如下：

L_i＝(x_A-x_B,y_A-y_B,z_A-z_B,α_A-α_B,β_A-β_B,γ_A-γ_B)。

S320对第一运动学模型中每根连接轴的长度求导，得到各个驱动电机的速度、位姿差和速度差的对应关系作为第二运动学模型。

示例性地，第二运动学模型如下：

其中H是主动减震机构运动学雅可比速度矩阵，其中

是动平台的第一期望速度。

S330根据第一运动学模型、第二运动模型、位姿差和速度差得到各个驱动电机的速度作为对应的每根连接轴的第二期望速度，每根连接轴的长度作为第二期望位置。

本实施例提供的主动减震机构控制方法提供一种根据主动设置的自由度，预设的动平台的期望位姿和期望速度的方法，使本发明可以根据主动选择的自由度，在该自由度上进行减震；并且通过主动减震机构的运动学逆解和主动减震机构的构型得到第一运动学模型和第二运动学模型，并根据位姿差和速度差结合两个运动学模型，计算每根连接轴的期望位置和期望速度。通过运动学逆解进行姿态控制，由于逆解算法简单且有精准的解析解的形式，可以避免主动减震机构的运动学的正解会因为其构型的不同，无法得出精确的解析解的情况。采用本方法提高每根连接轴的期望位置和期望速度的计算效率，保证主动减震平台对外界环境变化实时响应。

实施例4

基于实施例1～3中任意一个实施例，如图6所示，本发明提供还一种主动减震机构控制方法，其中步骤S400获取连接结构中每根连接轴的第二实时速度和第二实时位置、每根连接轴与驱动电机的传动关系，以及连接结构的动力学模型，并计算得到每根连接轴的力矩前馈，具体包括：

S410获取连接结构中每根连接轴的实时出力与连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项的关系作为动力学模型。

具体地，通过虚功率法，得到连接结构在关节空间下的动力学模型。对于一个具有n个自由度的的操作臂来说，它的所有连杆位置可由一组n个关节变量来确定。这样的一组变量通常被称为n×1的关节矢量。所有关节矢量组成的空间称为关节空间。

示例性地，动力学模型如下：

其中

为每个轴的实时出力，

表示关节空间下的惯性力项，G(l_i)表示关节空间下的重力项，

表示关节空间下的科氏力与离心力项，

表示关节空间下的摩擦力与阻尼力项。

S420获取关节空间内每根连接轴的实际位置和轴向上的实际速度，并计算得到每根连接轴轴向上的实际加速度。

具体地，获取连接机构中每个轴可以实时反馈实际的轴向上的速度与位置信息。通过对其反馈速度信息微分处理，可以得到每个轴的轴向实际加速度，速度，位置信息。

S430根据连接结构中每根连接轴的实际速度、实际位置和实际加速度，计算连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项。

S440根据连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项和动力学模型，计算连接结构中每根连接轴的实时出力。

S450获取每根连接轴与驱动电机的传动比，并根据传动比和每根连接轴的实际出力计算每根连接轴的力矩前馈。

具体地，根据每根连接轴轴与驱动电机的传动关系，η_i为电机的传动比，得到各个轴上的力矩前馈f_i＝η_i×τ_i。

步骤S500根据连接结构中每根连接轴的第二期望位置、第二期望速度和力矩前馈控制每根连接轴对应的驱动电机，具体包括：

S510将第二期望位置、第二期望速度和力矩前馈导入各个驱动电机的控制框架中，生成各个驱动电机对应的PID参数。

具体地，在驱动电机的控制框架的速度环中加入第二期望速度与第二期望位置的基础上，将计算出的力矩输出的前馈值与控制器给定值在电流环中的控制输出进行叠加，并输送至各轴对应的驱动电机中，并实时刷新力矩前馈值，从而进行力矩前馈补偿。

S520根据各个PID参数控制对应的驱动电机。

本实施例提供的主动减震机构控制方法通过主动减震机构的动力学模型、每根连接轴的实际位置和轴向上的实际速度，通过计算每根连接轴的实时出力作为中间量，得到连接轴的力矩前馈。并通过前馈力矩的方式优化主动减震平台在应对高频振动时的表现，使驱动部分的反应速度可以跟随高频激励，并将每根连接轴的期望位置、期望速度和力矩前馈导入驱动电机的控制框架中，实时更新各个驱动电机对应的PID参数，通过驱动电机控制每根轴的运动，使主动减震平台处于稳定状态，提高整体减振效果。

实施例5

本发明的另一个实施例，如图7所示，本发明还提供一种主动减震机构控制系统，其特征在于，应用于控制多自由度的主动减振平台，主动减震平台包括动平台、定平台，以及连接动平台与定平台的连接结构，连接结构包括若干根连接轴，每根连接轴均对应一驱动电机，包括第一获取模块10、第一计算模块20、第二计算模块30、第三计算模块40和控制模块50。

其中第一获取模块10用于定平台的实时位姿与实时速度。

具体地，获取定平台在惯性坐标系下俯仰角、扭转角和颠簸角的实时角速度以及横向、竖向和纵向的实时加速度，并计算得到定平台的第一实时位姿与第一实时速度。

主动减振平台的动平台相对于定平台具有多个自由度，常见的自由度为俯仰角、扭转角和颠簸角以及横向、竖向和纵向六个自由度，动平台相对于定平台也可以在其他方向上存在自由度，在其他方向上的自由度与这六个方向上的自由度控制方法相同。

由于主动减震平台是根据实时运动受力情况进行实时的减震调整，因此需要实时获取实时加速度和实时角速度，便于计算实时位姿和实时速度。

第一计算模块20与第一获取模块10连接，用于分别根据预设的动平台的第一期望位姿、第一期望速度、定平台的第一实时位姿和第一实时速度，得到动平台和定平台的位姿差和速度差。

具体地，第一计算模块20根据实际期望的减震效果，预设动平台的第一期望位姿和第一期望速度，并将第一期望位姿和第一期望速度与定平台实时检测到的第一实时位姿和第一实时速度作差，得到动平台和定平台的位姿差和速度差。

第二计算模块30与第一计算模块20连接，用于根据位姿差、速度差，以及连接结构的运动学模型，计算动平台平衡时连接结构中每根连接轴的第二期望位置与第二期望速度。

具体地，第二计算模块30预先根据主动减震平台的构型获取其连接结构的运动学模型。建立位姿差、速度差和连接结构中每根连接轴的第二期望位置与第二期望速度的对应关系。

将计算得到的动平台和定平台的位姿差和速度差代入运动学模型中，得到第二期望位置与第二期望速度。

第三计算模块40用于获取连接结构中每根连接轴的第二实时速度和第二实时位置、每根连接轴与驱动电机的传动关系，以及连接结构的动力学模型，并计算得到每根连接轴的力矩前馈。

具体地，由于每根连接轴均与对应的驱动电机连接，因此通过每根连接轴对应的驱动电机可以获取第二实时速度和第二实时位置。

第三计算模块40根据主动减震平台的结构，获取其动力学模型，将第二实时速度和第二实时位置代入动力学模型中，代入动力学模型中可以得到每根轴的实际出力，并通过每根轴与驱动电机的传动比，计算得到每根连接轴的力矩前馈。

控制模块50与第二计算模块30和第三计算模块40连接，用于根据连接结构中每根连接轴的第二期望位置、第二期望速度和力矩前馈控制每根连接轴对应的驱动电机。

本实施例提供的主动减震机构控制系统通过动平台与定平台之间的位姿差和速度差，结合主动减震机构的运动学模型和动力学模型，计算动平台与定平台之间每根连接轴的期望位姿、期望速度以及力矩前馈，根据期望位姿、期望速度以及力矩前馈控制每个连接轴的驱动电机，使主动减震平台处于平衡状态，减少外力干扰对于主动减震平台稳定性的影响。

实施例6,

基于实施例5，如图8所示，本发明还提供一种主动减震机构控制系统，其中第三计算模块40包括动力学模型获取单元41、第一计算单元42、第二计算单元43、第三计算单元44和第四计算单元45。

其中动力学模型获取单元41，用于获取连接结构中每根连接轴的实时出力与连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项的关系作为动力学模型。

具体地，动力学模型获取单元41通过虚功率法，得到连接结构在关节空间下的动力学模型。对于一个具有n个自由度的的操作臂来说，它的所有连杆位置可由一组n个关节变量来确定。这样的一组变量通常被称为n×1的关节矢量。所有关节矢量组成的空间称为关节空间。

示例性地，动力学模型如下：

其中

为每个轴的实时出力，

表示关节空间下的惯性力项，G(l_i)表示关节空间下的重力项，

表示关节空间下的科氏力与离心力项，

表示关节空间下的摩擦力与阻尼力项。

第一计算单元42，用于获取关节空间内每根连接轴的实际位置和轴向上的实际速度，并计算得到每根连接轴轴向上的实际加速度。

具体地，第一计算单元42获取连接机构中每个轴可以实时反馈实际的轴向上的速度与位置信息。通过对其反馈速度信息微分处理，可以得到每个轴的轴向实际加速度，速度，位置信息。

第二计算单元43，与第一计算单元42连接，用于根据连接结构中每根连接轴的实际速度、实际位置和实际加速度，计算连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项。

第三计算单元44，与动力学模型获取单元41和第二计算单元43连接，用于根据连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项和动力学模型，计算连接结构中每根连接轴的实时出力。

第四计算单元45，与第三计算单元44连接，用于获取每根连接轴与驱动电机的传动比，并根据传动比和每根连接轴的实际出力计算每根连接轴的力矩前馈。

具体地，第四计算单元45根据每根连接轴轴与驱动电机的传动关系，η_i为电机的传动比，得到各个轴上的力矩前馈f_i＝η_i×τ_i。

本实施例提供的主动减震机构控制方法通过主动减震机构的动力学模型、每根连接轴的实际位置和轴向上的实际速度，通过计算每根连接轴的实时出力作为中间量，得到连接轴的力矩前馈，并通过前馈力矩的方式优化主动减震平台在应对高频振动时的表现，使驱动部分的反应速度可以跟随高频激励，提高整体减振效果。

实施例7

本发明的一个实施例，一种存储介质，存储介质中存储有至少一条指令，指令由处理器加载并执行以实现实施例1～5中任意一项提供的主动减震机构控制方法所执行的操作。例如，存储介质可以是只读内存(ROM)、随机存取存储器(RAM)、只读光盘(CD-ROM)、磁带、软盘和光数据存储设备等。

它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述或记载的部分，可以参见其他实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

在本申请所提供的实施例中，应该理解到，所揭露的一种主动减震机构控制方法、系统和存储介质，可以通过其他的方式实现。例如，以上所描述的一种主动减震机构控制方法、系统和存储介质实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如，多个单元或模块可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的通讯连接可以是通过一些接口，装置或单元的通讯连接或集成电路，可以是电性、机械或其他的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可能集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

应当说明的是，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种主动减震机构控制方法，其特征在于，应用于控制多自由度的主动减振平台，所述主动减震平台包括动平台、定平台，以及连接所述动平台与所述定平台的连接结构，所述连接结构包括若干根连接轴，每根所述连接轴均对应一驱动电机，包括步骤：

获取所述定平台的第一实时位姿与第一实时速度；

分别根据预设的所述动平台的第一期望位姿、第一期望速度，以及所述定平台的所述第一实时位姿和所述第一实时速度，得到所述动平台和所述定平台的位姿差和速度差；

根据所述位姿差、所述速度差，以及所述连接结构的运动学模型，计算所述动平台平衡时所述连接结构中每根所述连接轴的第二期望位置与第二期望速度；

根据获取的所述连接结构中每根所述连接轴的第二实时速度和第二实时位置，每根所述连接轴与所述驱动电机的传动关系，以及所述连接结构的动力学模型，计算每根所述连接轴的力矩前馈；

根据所述连接结构中每根所述连接轴的所述第二期望位置、所述第二期望速度和所述力矩前馈，控制每根所述连接轴对应的所述驱动电机。

2.根据权利要求1所述的一种主动减震机构控制方法，其特征在于，所述的计算所述定平台的第一实时位姿与第一实时速度，具体包括：

对所述实时加速度进行低通滤波处理，对所述实时角速度进行高通滤波处理；

根据所述定平台的随体坐标系与惯性坐标系之间的旋转矩阵，得到所述定平台初始时刻的初始俯仰角、初始颠簸角和初始扭转角；

根据所述初始俯仰角、所述初始颠簸角、所述初始扭转角、所述实时角速度和所述实时加速度，得到所述定平台的实时俯仰角、实时颠簸角和实时扭转角；

根据所述实时俯仰角、所述实时颠簸角和所述实时扭转角，计算所述第一实时位姿和所述第一实时速度。

3.根据权利要求2所述的一种主动减震机构控制方法，其特征在于：

所述主动减震机构在俯仰角、扭转角和颠簸角三个自由度上包括若干个耦合自由度和若干个独立自由度；

所述的对所述实时加速度进行低通滤波处理，对所述实时角速度进行高通滤波处理之后，还包括：

根据所述旋转矩阵，得到所述定平台在初始时刻时，若干个所述独立自由度对应的初始角度；

根据若干个所述初始角度、所述实时角速度和所述实时加速度，计算所述定平台的若干个所述独立自由度对应的实时角度；

根据若干个所述独立自由度对应的实时角度，计算若干个所述耦合自由度对应的实时角度；

根据若干个所述独立自由度对应的实时角度和若干个所述耦合自由度对应的实时角度，计算所述第一实时位姿和所述第一实时速度。

4.根据权利要求1所述的一种主动减震机构控制方法，其特征在于，所述的计算所述定平台的第一实时位姿与第一实时速度之后，所述的得到所述动平台和所述定平台的位姿差和速度差之前，还包括：

选取所述动平台在俯仰角、扭转角、颠簸角、横向、竖向和纵向六个自由度中的若干个减振自由度和若干个非减震自由度；

设置所述第一期望位姿在若干个所述减振自由度上的第一期望位姿值和第一期望速度值为0，在若干个所述非减震自由度上的所述第一期望位姿值和所述第一期望速度值，与所述定平台在对应的自由度上的所述实时位姿值和所述实时速度值相同，得到所述第一期望位姿和所述第一期望速度。

5.根据权利要求1所述的一种主动减震机构控制方法，其特征在于，所述的根据所述位姿差、所述速度差，以及所述连接结构的运动学模型，计算所述动平台平衡时所述连接结构中每根所述连接轴的第二期望位置与第二期望速度，具体包括：

根据所述主动减震机构的构型，以及所述主动减震机构的运动学逆解，得到每根所述连接轴的长度与所述位姿差之间的换算比例，将所述换算比例作为第一运动学模型；

根据所述第一运动学模型，得到各个所述驱动电机的速度、所述位姿差和所述速度差之间的对应关系，将所述对应关系作为第二运动学模型；

根据所述第一运动学模型、所述第二运动模型、所述位姿差和所述速度差计算各个所述驱动电机的速度作为各个所述驱动电机对应的所述连接轴的所述第二期望速度，计算每根所述连接轴的长度作为所述第二期望位置。

6.根据权利要求1所述的一种主动减震机构控制方法，其特征在于，所述的根据获取的所述连接结构中每根所述连接轴的第二实时速度和第二实时位置，每根所述连接轴与所述驱动电机的传动关系，以及所述连接结构的动力学模型，计算每根所述连接轴的力矩前馈，具体包括：

获取所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力、阻尼力项和每根所述连接轴的实时出力之间的关系作为所述动力学模型；

获取每根所述连接轴的所述实际位置和轴向上的所述实际速度，并计算得到每根所述连接轴轴向上的实际加速度；

根据每根所述连接轴的所述实际速度、所述实际位置和所述实际加速度，计算所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项；

根据所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力、阻尼力项和所述动力学模型，计算每根所述连接轴的所述实时出力；

获取每根所述连接轴与所述驱动电机的传动比，并根据所述传动比和每根所述连接轴的所述实际出力，计算每根所述连接轴的所述力矩前馈。

7.根据权利要求1所述的一种主动减震机构控制方法，其特征在于，所述的根据所述连接结构中每根所述连接轴的所述第二期望位置、所述第二期望速度和所述力矩前馈，控制每根所述连接轴对应的所述驱动电机，具体包括：

将所述第二期望位置、所述第二期望速度和所述力矩前馈导入各个所述驱动电机的控制框架中，生成各个所述驱动电机对应的PID参数；

根据各个所述PID参数控制对应的所述驱动电机。

8.一种主动减震机构控制系统，其特征在于，应用于控制多自由度的主动减振平台，所述主动减震平台包括动平台、定平台，连接所述动平台与所述定平台的连接结构，所述连接结构包括若干根连接轴，每根所述连接轴均对应一驱动电机，包括：

第一获取模块，用于获取所述定平台的第一实时位姿与第一实时速度；

第一计算模块，与所述第一获取模块连接，用于分别根据预设的所述动平台的第一期望位姿、第一期望速度，以及所述定平台的所述第一实时位姿和所述第一实时速度，得到所述动平台和所述定平台的位姿差和速度差；

第二计算模块，与所述第一计算模块连接，用于根据所述位姿差、所述速度差，以及所述连接结构的运动学模型，计算所述动平台平衡时所述连接结构中每根所述连接轴的第二期望位置与第二期望速度；

第三计算模块，用于根据获取的所述连接结构中每根所述连接轴的第二实时速度和第二实时位置，每根所述连接轴与所述驱动电机的传动关系，以及所述连接结构的动力学模型，计算每根所述连接轴的力矩前馈；

控制模块，与所述第二计算模块和所述第三计算模块连接，用于根据所述连接结构中每根所述连接轴的所述第二期望位置、所述第二期望速度和所述力矩前馈，控制每根所述连接轴对应的所述驱动电机。

9.根据根据权利要求8所述的一种主动减震机构控制系统，其特征在于，所述第三计算模块包括：

动力学模型获取单元，用于获取所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力、阻尼力项和每根所述连接轴的实时出力之间的关系作为所述动力学模型；

第一计算单元，用于获取每根所述连接轴的所述实际位置和轴向上的所述实际速度，并计算得到每根所述连接轴轴向上的实际加速度；

第二计算单元，与所述第一计算单元连接，用于根据每根所述连接轴的所述实际速度、所述实际位置和所述实际加速度，计算所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力与阻尼力项；

第三计算单元，与所述动力学模型获取单元和所述第二计算单元连接，用于根据所述连接结构在关节空间的惯性力项、重力项、科氏力、离心力项、摩擦力、阻尼力项和所述动力学模型，计算每根所述连接轴的所述实时出力；

第四计算单元，与所述第三计算单元连接，用于获取每根所述连接轴与所述驱动电机的传动比，并根据所述传动比和每根所述连接轴的所述实际出力，计算每根所述连接轴的所述力矩前馈。

10.一种存储介质，其特征在于：所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现如权利要求1～7中任意一项所述的主动减震机构控制方法所执行的操作。

Images