CN113955032B - 一种主动减少晃动的力控方法及系统、自平衡装置 - Google Patents

Abstract

本发明提供了一种主动减少晃动的力控方法及系统、自平衡装置，所述主动减少晃动的力控方法，用于在载具行驶过程中减少载具内支撑件的晃动，包括：获取载具行驶过程中在第一时刻的第一载具振动信息和第一支撑件振动信息；基于载具和支撑件之间的动力学关系、第一载具振动信息和第一支撑件振动信息，确定任务空间的第一控制力；基于第一控制力控制执行机构带动支撑件相对于载具运动，以在载具行驶过程中减少支撑件的晃动。本发明基于控制力主动控制执行机构动作，可以减少载具行驶过程中支撑件的晃动，提高支撑件上放置物品的平稳性，同时使控制过程中施加的控制力更柔顺；针对用户对象，更柔顺的控制力使用户体验更好。

Description

一种主动减少晃动的力控方法及系统、自平衡装置

技术领域

本发明涉及控制领域，尤指一种主动减少晃动的力控方法及系统、自平衡装置。

背景技术

在船舶出海航行时，因恶劣海况下剧烈风浪的影响，船舶会产生严重的低频大幅振动。其振动主要为横摇和纵摇。横摇为船舶左右摇摆，纵摇则是船舶前后摇摆，垂荡为船舶上下起伏。若遇到较大的风浪，船舶会横向、纵向同时摇晃，使舰艇发生不规律的摇晃，甚至是抖动。5级海况下，1000吨左右的船只，其横摇角可达到20°，纵摇角可达到10°，垂荡幅值可达到1m。这些摇摆和振动会导致船上人员出现晕动症，包括呼吸困难、冒冷汗、恶心呕吐等，严重者甚至危及生命。

传统的船舶减摇装置(减摇水舱、减摇舵、减摇陀螺、减摇鳍、减摇重块等)体积重量大、功耗大、成本高，主要用于减缓船舶整体的横摇，船舶经减摇装置处理后依然有较大的摇摆，无法完全解决晕船等问题。

目前有利用机器人机构构建自平衡平台，比如防晕床，利用机器人机构的体积小、负载高的优点，结合机器人的实时控制技术消除横摇和纵摇，进而有效防治晕船症。但是采用的控制策略几乎都是传统的位置控制技术(简称位控)，即控制机器人的末端执行器的位置。在某些应用场合中，更加精确地控制施加在末端执行器的力比控制末端执行器的位置更加重要。比如，在船舶上实施医疗救治，当患者躺在防晕平台上时，就需要考虑防晕平台与患者的接触，严格控制防晕平台中机器人的末端执行器的力的大小，防止在控制平台晃动的过程中引入对患者的意外伤害。

发明内容

本发明针对现有技术的不足，提供了一种主动减少晃动的力控方法及系统、自平衡装置。

本发明提供的技术方案如下：

一种主动减少晃动的力控方法，用于在载具行驶过程中减少载具内支撑件的晃动，包括：获取载具行驶过程中在第一时刻的第一载具振动信息和第一支撑件振动信息；基于所述载具和所述支撑件之间的动力学关系、所述第一载具振动信息和所述第一支撑件振动信息，确定任务空间的第一控制力；基于所述第一控制力控制执行机构带动所述支撑件相对于所述载具运动，以在所述载具行驶过程中减少所述支撑件的晃动。

在一些实施例中，还包括：

获取载具行驶过程中在第二时刻的第二载具振动信息和第二支撑件振动信息；

获取完成与所述第一控制力对应的第一执行动作后的所述执行机构的关节空间位移；

根据所述执行机构的关节空间位移，进行运动学正解得到所述载具和所述支撑件之间的相对运动信息；

基于所述载具和所述支撑件之间的动力学关系、所述第二载具振动信息、所述第二支撑件振动信息以及所述载具和所述支撑件之间的相对运动信息，确定所述任务空间的第二控制力；

基于所述第二控制力控制所述执行机构带动所述支撑件相对于所述载具运动。

在一些实施例中，所述的基于所述载具和所述支撑件之间的动力学关系、所述第二载具振动信息、所述第二支撑件振动信息以及所述载具和所述支撑件之间的相对运动信息，确定所述任务空间的第二控制力，包括：

基于所述第二载具振动信息、所述第二支撑件振动信息，以及所述载具和所述支撑件之间的相对运动信息，确定横摇的控制率、纵摇的控制率和垂荡的控制率；

基于所述载具和所述支撑件之间的动力学关系、所述横摇的控制率、所述纵摇的控制率和所述垂荡的控制率，确定所述任务空间的第二控制力。

在一些实施例中，采用PID控制算法确定所述横摇的控制率和所述纵摇的控制率，采用弹簧阻尼和天棚阻尼控制算法确定所述垂荡的控制率。

在一些实施例中，所述的采用PID控制算法确定所述横摇的控制率和所述纵摇的控制率，采用弹簧阻尼和天棚阻尼控制算法确定所述垂荡的控制率，包括：采用下述公式计算横摇的控制率纵摇的控制率/>和垂荡的控制率/>

其中，为所述第二载具振动信息中的垂向加速度，/>为第二载具振动信息中的横摇角加速度，/>为第二载具振动信息中的纵摇角加速度，/>为所述第二支撑件振动信息中的垂向速度，α_B为所述第二支撑件振动信息中的横摇角，/>为所述第二支撑件振动信息中的横摇角速度，γ_B为所述第二支撑件振动信息中的纵摇角，/>为所述第二支撑件振动信息中的纵摇角速度，Δy为所述相对运动信息中的垂向位移，/>为所述相对运动信息中的垂向速度，k_y、c_y、s_y、k_α、c_α、k_γ、c_γ为相关参数。

在一些实施例中，采用以下公式计算任务空间的第二控制力f_c：

其中，ΔX为所述相对运动信息中的相对位移信息，所述相对运动信息中的相对速度信息，/>为任务空间的惯性力项，/>为任务空间的柯氏力与离心力项，η(ΔX)为任务空间的重力项，/>为所述垂荡的控制率，/>为所述横摇的控制率，为所述纵摇的控制率。

在一些实施例中，采用安装于所述载具的惯导单元获取所述载具的所述第一载具振动信息和所述第二载具振动信息；采用安装于所述支撑件的惯导单元获取所述支撑件的所述第一支撑件振动信息和所述第二支撑件振动信息。

在一些实施例中，所述的基于所述第二控制力控制所述执行机构带动所述支撑件相对于所述载具运动，包括：根据所述任务空间的第二控制力，进行运动学逆解，得到所述执行机构在关节空间的控制力；

执行所述执行机构在关节空间的控制力对应的指令，所述执行机构带动所述支撑件相对于所述载具运动。

本发明还提供一种主动减少晃动的力控系统，用于在载具行驶过程中减少载具内支撑件的晃动，包括：

振动信息获取单元，用于获取载具行驶过程中在第一时刻的第一载具振动信息和第一支撑件振动信息；

控制力计算单元，用于基于所述载具和所述支撑件之间的动力学关系、所述第一载具振动信息和所述第一支撑件振动信息，确定任务空间的第一控制力；

执行控制单元，用于基于所述第一控制力控制执行机构带动所述支撑件相对于所述载具运动，以在所述载具行驶过程中减少所述支撑件的晃动。

本发明还提供一种自平衡装置，包括：

底座，固定于载具；

第一惯导单元，安装于所述底座或所述载具，用于检测所述载具的振动信息；

执行机构，采用机器人构型，包括驱动器；

编码器，安装于所述执行机构，用于检测所述执行机构的关节空间位移；

支撑件，所述执行机构安装于所述底座和所述支撑件之间；

第二惯导单元，安装于所述支撑件，用于检测所述支撑件的振动信息；

控制单元，与所述第一惯导单元、所述第二惯导单元、所述编码器、所述驱动器电性连接，所述控制单元包括控制器和存储器，所述存储器用于存储计算机程序，所述控制器用于运行所述计算机程序时实现前述的主动减少晃动的力控方法。

与现有技术相比，本发明所提供的一种主动减少晃动的力控方法及系统、自平衡装置具有以下至少一条有益效果：

1、本发明基于载具与支撑件之间的动力学关系、所检测的载具的振动信息和支撑件的振动信息，确定机器人的任务空间的控制力；基于该控制力主动控制执行机构动作，以减少在载具行驶过程中支撑件的晃动，提高支撑件上放置物品的平稳性，同时使控制过程中施加的控制力更柔顺。如果支撑件上坐着或躺着的对象为人，则更柔顺的控制力使用户体验更好。

2、本发明通过消除船舶的横摇角和纵摇角，同时进行垂向的加速度衰减，实现了姿态平衡和垂向减振的一体化，能够取得更好的防治晕船效果。

附图说明

下面将以明确易懂的方式，结合附图说明优选实施方式，对一种主动减少晃动的力控方法及系统、自平衡装置的上述特性、技术特征、优点及其实现方式予以进一步说明。

图1是本发明的主动减少晃动的力控方法的一个实施例的流程图；

图2是本发明的实施例的主动减少晃动的力控方法所建立的坐标系的示意图；

图3是本发明的主动减少晃动的力控系统的一个实施例的结构示意图；

图4是本发明的自平衡装置的一个实施例的结构示意图；

图5是本发明的自平衡装置的其他实施例的结构示意图；

图6和图7是本发明的优选实施例的主动减少晃动的力控方法的控制流程图；

图8是本发明的机器人防晕平台的垂向伯德图；

图9是本发明的机器人防晕平台的结构示意图；

图10是本发明的机器人防晕平台的仿真结果的上台面垂向位移曲线；

图11是本发明的机器人防晕平台的仿真结果的上台面横摇角曲线和纵摇角曲线。

具体实施方式

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

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

如前所述，采用机器人机构构建的自平衡平台，目前采用的控制策略主要是位控技术，具有较好的位置跟踪精度，但一般不具备力控制能力。本申请提供一种适用于位控机器人的力控制方法，不仅满足一些应用的需要，还可以在普通场景下，通过控制机器人的末端执行器的力的大小，在控制平台的晃动的同时使控制过程中施加的控制力更柔顺。

位控方法是以执行机构的关节目标位置作为控制输出量，而本申请的力控方法是以执行机构的末端执行器的力作为控制输出量。

下面采用实施例具体说明。

本发明的一个实施例，如图1所示，一种主动减少晃动的力控方法，用于在载具行驶过程中减少载具内支撑件的晃动，包括：

步骤S100获取载具行驶过程中在第一时刻的第一载具振动信息和第一支撑件振动信息；

步骤S200基于载具和支撑件之间的动力学关系、第一载具振动信息和第一支撑件振动信息，确定任务空间的第一控制力；

步骤S300基于第一控制力控制执行机构带动支撑件相对于载具运动，以在载具行驶过程中减少支撑件的晃动。

具体的，载具与支撑件设有执行机构，执行机构采用机器人构型，支撑件为机器人的末端。支撑件为机器人的任务空间或操作空间。

在本方法中，基于载具与支撑件之间的动力学关系、所检测的载具的振动信息和支撑件的振动信息，确定机器人的任务空间的控制力；基于该控制力主动控制执行机构动作，以减少在载具行驶过程中支撑件的晃动，提高支撑件上放置物品的平稳性，同时使控制过程中施加的控制力更柔顺。如果支撑件上坐着或躺着的对象为人，则更柔顺的控制力使用户体验更好。

载具可以是船舶、汽车、飞机等交通工具。支撑件可以是座椅、桌子或床等。可以理解的是，载具与支撑件的具体类型不应当构成对本发明的限制。

在一些实施例中，所述主动减少晃动的力控方法还包括：

步骤S400获取载具行驶过程中在第二时刻的第二载具振动信息和第二支撑件振动信息；

步骤S500获取完成与第一控制力对应的第一执行动作后的执行机构的关节空间位移；

步骤S510根据执行机构的关节空间位移，进行运动学正解得到载具和支撑件之间的相对运动信息；

步骤S600基于载具和支撑件之间的动力学关系、第二载具振动信息、第二支撑件振动信息以及载具和支撑件之间的相对运动信息，确定任务空间的第二控制力；

步骤S700基于第二控制力控制执行机构带动支撑件相对于载具运动。

在本优选实施例中，在确定第二时刻对应的任务空间的第二控制力的过程中，在与第一控制力对应的第一执行动作的基础上进行确定，可以提高第二控制力的精准度。第一执行动作是执行机构基于第一控制力而执行的动作。

在步骤S100和步骤S400中，采用安装于载具的惯导单元获取载具的第一载具振动信息和第二载具振动信息，采用安装于支撑件的惯导单元获取支撑件的第一支撑件振动信息和第二支撑件振动信息。

惯导单元包括三轴加速度计、三轴陀螺仪以及三轴磁力计，能够检测惯导单元的安装载体的振动信息，振动信息包括但不限于随体系三轴加速度和随体系三轴角速度，经过进一步的计算处理(比如积分)能够获得安装载体的实时的位置、速度、角度信息。在一些实施例中，惯导单元还包括GPS，以增强感知精度。

由于传感器本身的偏差和噪声，传感器输出的数据并不精确，如果直接积分会导致快速发散。在一些实施例中，通过滤波算法消除振动信息中的偏差和噪声，然后再进行积分，可以得到高精度的安装载体的姿态信息，包括实时的横摇角、纵摇角以及垂向加速度。

在步骤S500中，采用设置于执行机构中的编码器或位置/速度感应器获取完成与第一控制力对应的第一执行动作后的执行机构的关节空间位移。根据执行机构的关节空间位移，进行运动学正解得到载具和支撑件之间的相对运动信息。

如图2所示，在载具和支撑件上选一位置，分别建立随体坐标系O_Ax_Ay_Az_A和随体坐标系O_Bx_By_Bz_B。支撑件为机器人的任务空间，其相对于载具的6维运动为Δx,Δy,Δz,Δα,Δβ,Δγ，前三维为相对移动，后三维为相对转动。对于自由度为n的机器人，任务空间中只有n个独立运动，其它运动为耦合运动。

假设连接载具和支撑件的执行机构的关节空间坐标为q₁,q₂,…,q_n，分别代表第1，2，…，n关节的运动。根据几何学和机器人运动学知识，可以推导出此机器人的运动学正解为：

ΔX＝f_FWD(q)，

其中，q＝[q₁,q₂,...,q_n]^T，ΔX＝[Δx,Δy,Δz,Δα,Δβ,Δγ]^T。

ΔX、分别为载具和支撑件之间的相对运动信息中的相对位移信息和相对速度信息，q、/>分别为关节空间位移和关节空间速度，J为逆运动学的速度映射雅克比矩阵。

在一些实施例中，步骤S600包括：

步骤S610基于第二载具振动信息、第二支撑件振动信息，以及载具和支撑件之间的相对运动信息，确定横摇的控制率、纵摇的控制率和垂荡的控制率。

在任务空间设置控制率，使得支撑件在惯性空间的横摇角α_B和纵摇角γ_B趋近于0，并使垂向加速度有限衰减。

在一些实施例中，采用PID控制算法确定横摇的控制率和纵摇的控制率，采用弹簧阻尼和天棚阻尼控制算法确定垂荡的控制率，具体可采用下述公式计算横摇的控制率纵摇的控制率/>和垂荡的控制率/>

其中，分别为第二载具振动信息中的垂向加速度、横摇角加速度和纵摇角加速度，/>α_B、/>γ_B、/>分别为第二支撑件振动信息中的垂向速度、横摇角、横摇角速度、纵摇角和纵摇角速度，Δy、/>分别为载具与支撑件之间的相对运动信息中的垂向位移和垂向速度，k_y、c_y、s_y、k_α、c_α、k_γ、c_γ为相关参数。

步骤S620基于载具和支撑件之间的动力学关系、横摇的控制率、纵摇的控制率和垂荡的控制率，确定任务空间的第二控制力。

记载具在惯性空间的绝对运动为：X_A＝[x_A,y_A,z_A,α_A,β_A,γ_A]^T。

则支撑件在惯性空间的绝对运动为：

X_B＝X_A+ΔX，X_B＝[x_B,y_B,z_B,α_B,β_B,γ_B]^T。

支撑件在任务空间的动力学方程为：

其中，等号前第一项为任务空间的惯性力项，第二项为任务空间的柯氏力与离心力项，第三项为任务空间的重力项；等号后f_c为任务空间的控制力，它和关节控制力u的关系为：

f_c＝J^Tu。

在获得横摇的控制率纵摇的控制率/>和垂荡的控制率/>后，将代入上述动力学方程，确定任务空间的第二控制力f_c。

在一些实施例中，步骤S700包括：

步骤S710根据第二控制力，进行运动学逆解，得到执行机构在关节空间的控制力。

根据公式u＝J^-Tf_c，得到与任务空间的控制力f_c对应的关节空间的控制力u。

步骤S720执行执行机构在关节空间的控制力对应的指令，执行机构带动支撑件相对于载具运动。

本发明的一个实施例，如图3所示，一种主动减少晃动的力控系统，用于在载具行驶过程中减少载具内支撑件的晃动，包括：

振动信息获取单元100，用于获取载具行驶过程中在第一时刻的第一载具振动信息和第一支撑件振动信息；

控制力计算单元200，用于基于载具和支撑件之间的动力学关系、第一载具振动信息和第一支撑件振动信息，确定任务空间的第一控制力；

执行控制单元300，用于基于第一控制力控制执行机构带动支撑件相对于载具运动，以在载具行驶过程中减少支撑件的晃动。

在一些实施例中，还包括：

信息反馈单元400，用于获取完成与第一控制力对应的第一执行动作后的执行机构的关节空间位移；根据执行机构的关节空间位移，进行运动学正解得到载具和支撑件之间的相对运动信息；

振动信息获取单元100，还用于获取载具行驶过程中在第二时刻的第二载具振动信息和第二支撑件振动信息；

控制力计算单元200，还用于基于载具和支撑件之间的动力学关系、第二载具振动信息、第二支撑件振动信息以及载具和支撑件之间的相对运动信息，确定任务空间的第二控制力；

执行控制单元300，还用于基于第二控制力控制执行机构带动支撑件相对于载具运动。

振动信息获取单元中，采用安装于载具的惯导单元获取载具的第一载具振动信息和第二载具振动信息，采用安装于支撑件的惯导单元获取支撑件的第一支撑件振动信息和第二支撑件振动信息。

惯导单元包括三轴加速度计、三轴陀螺仪以及三轴磁力计，能够检测所述惯导单元的安装载体的振动信息，振动信息包括但不限于随体系三轴加速度和随体系三轴角速度，经过进一步的计算处理能够获得安装载体的实时的位置、速度、角度信息。

信息反馈单元400中，采用设置于执行机构中的编码器或位置/速度感应器获取完成与第一控制力对应的第一执行动作后的执行机构的关节空间位移。

在一些实施例中，控制力计算单元200，还用于基于第二载具振动信息、第二支撑件振动信息，以及载具和支撑件之间的相对运动信息，确定横摇的控制率、纵摇的控制率和垂荡的控制率；基于载具和支撑件之间的动力学关系、横摇的控制率、纵摇的控制率和垂荡的控制率，确定任务空间的第二控制力。

在一些实施例中，执行控制单元300，还用于根据任务空间的第二控制力，进行运动学逆解，得到执行机构在关节空间的控制力；执行执行机构在关节空间的控制力对应的指令，执行机构带动支撑件相对于载具运动。

需要说明的是，本发明提供的主动减少晃动的力控系统的实施例与前述提供的主动减少晃动的力控方法的实施例均基于同一发明构思，能够取得相同的技术效果。因而，主动减少晃动的力控系统的实施例的其它具体内容可以参照前述主动减少晃动的力控方法的实施例内容的记载。

本发明的一个实施例，如图4所示，一种自平衡装置，包括：

底座10，固定于载具；

第一惯导单元11，安装于底座或载具，用于检测载具的振动信息；

执行机构20，采用并联机器人或串并混联机器人构型，包括驱动器21；

编码器22，安装于执行机构，用于检测执行机构的关节空间位移；

支撑件30，执行机构20安装于底座和支撑件之间；

第二惯导单元31，安装于支撑件，用于检测支撑件的振动信息；

控制单元40，与第一惯导单元11、第二惯导单元31、编码器22、驱动器21电性连接，控制单元包括控制器和存储器，存储器用于存储计算机程序，控制器用于运行计算机程序时实现如前所述的主动减少晃动的力控方法。

自平衡装置可根据需要有3、4、...n个执行结构，如图5所示，本实施例对此不做限制。

本发明还提供了一个具体应用场景实施例，将前述的主动减少晃动的力控方法与系统应用于船舶上的机器人防晕平台。

该防晕平台由6个部分组成：①底座；②第一惯导单元；③上台面；④第二惯导单元；⑤机器人机构；⑥驱控单元。

①底座为刚体，固定于船舱地板上或甲板上。

②第一惯导单元包括3轴加速度计、3轴陀螺仪和3轴磁力计，固定于底座上，可感知船舶的振动信息，经算法处理，可以得到船舶实时的位置、速度、角度信息。在惯导单元中可增加GPS以增强感知精度。

③上台面是与人体接触的表面，也是机器人控制系统的任务空间和控制目标，期望其在惯性空间的姿态保持恒定，并且垂向加速度尽可能衰减。

④第二惯导单元包括3轴加速度计、3轴陀螺仪、3轴磁力计、GPS(选配)，固定于上台面上，可感知上台面实时的位置、速度、角度信息。

⑤机器人机构可采用并联机器人、串联机器人或串并混联机器人构型，但至少应具备横摇、纵摇、垂荡3个自由度，可采用图5的几种常见机器人构型，此处仅举例，并不代表本发明所提出的控制算法只适用这几种机器人构型。机器人机构包括n个执行单元，n为机器人自由度，执行单元可选用电缸电机。

⑥驱控单元为系统的核心，包括控制单元。如图6所示，控制单元根据第一惯导单元、第二惯导单元、执行机构编码器等信息，制定控制率，获得任务空间的控制力f_c(即图6的Fc)，形成驱动指令，并通过驱动器下发给每个执行单元执行。

如图7所示，具体实施如下：

(1)通过第一惯导单元获取船舶的实时姿态信息，通过第二惯导单元获取平台上台面的实时姿态信息。

安装在底座的第一惯导单元和安装在上台面的第二惯导单元分别测量船舶的振动信息和上台面的振动信息，包括3轴加速度和3轴角速度。

可通过滤波算法消除传感器所测量数据中的偏差和噪声，然后再进行积分，得到高精度的船舶和上台面的姿态信息，包括实时的横摇角、纵摇角以及垂向加速度。

(2)机器人运动学分析

如图2所示，在机器人防晕平台的底座和上台面的中心位置，分别建立随体坐标系O_Ax_Ay_Az_A和随体坐标系O_Bx_By_Bz_B。上台面为机器人的任务空间或操作空间，其相对于底座的6维运动为Δx,Δy,Δz,Δα,Δβ,Δγ，前三维为相对移动，后三维为相对转动。对于自由度为n的机器人，任务空间的6维运动中只有n个独立运动，其它运动为耦合运动。

假设连接载具和支撑件的执行机构的关节空间坐标为q₁,q₂,…,q_n，分别代表第1，2，…，n关节的运动。根据几何学和机器人运动学知识，可以推导出此机器人的运动学正解为：

ΔX＝f_FWD(q)，

其中，q＝[q₁,q₂,...,q_n]^T，ΔX＝[Δx,Δy,Δz,Δα,Δβ,Δγ]^T。

ΔX、分别为载具和支撑件之间的相对运动信息中的相对位移信息和相对速度信息，q、/>分别为关节空间位移和关节空间速度，J为逆运动学的速度映射雅克比矩阵。

(3)任务空间(操作空间)动力学方程

记底座(船舶)在惯性空间的绝对运动为：X_A＝[x_A,y_A,z_A,α_A,β_A,γ_A]^T。

则机器人防晕平台在惯性空间的绝对运动为：

X_B＝X_A+ΔX，X_B＝[x_B,y_B,z_B,α_B,β_B,γ_B]^T。

机器人防晕平台在任务空间的动力学方程为：

其中，等号前第一项为任务空间的惯性力项，第二项为任务空间的柯氏力与离心力项，第三项为任务空间的重力项；等号后f_c为任务空间的控制力，它和关节控制力u的关系为：

f_c＝J^Tu。

(4)任务空间(操作空间)控制率设计

在任务空间设置控制率，使得上台面在惯性空间的横摇角和纵摇角趋近于0，并使垂向加速度有限衰减。此处采用PID控制理论设计横摇角和纵摇角的控制率，采用弹簧阻尼+天棚阻尼控制垂向加速度的控制率。

根据下述公式分别计算横摇的控制率、纵摇的控制率和垂荡的控制率：

[1]

[2]

[3]

其中，分别为船舶的垂向加速度、横摇角加速度和纵摇角加速度，α_B、/>γ_B、/>分别为上台面的垂向速度、横摇角、横摇角速度、纵摇角和纵摇角速度，Δy、/>分别为底座与上台面之间的相对运动信息中的垂向位移和垂向速度，k_y、c_y、s_y、k_α、c_α、k_γ、c_γ为相关参数。Δy、/>初始值设为0。

第[2]式和第[3]式为标准的PID控制，合理调整PID参数，可以使横摇角α_B和纵摇角γ_B快速收敛到0值。

第[1]式的传递函数为：

其伯德图如图8所示，可知该控制率可以使机器人防晕平台的垂向加速度有效衰减。天棚阻尼的加入可以消除传统弹簧阻尼在低频段的放大及共振峰，使得可以在全频段实现高效减振。

(5)关节空间驱动指令解算

根据机器人的运动学逆解，可以得到机器人防晕平台在关节空间的控制力：u＝J^{- T}f_c。

(6)伺服驱动器根据设定的控制力指令执行，同时编码器将执行机构的实际执行情况实时反馈给控制系统。

重复上述步骤，直至上台面在惯性空间的横摇角和纵摇角趋近于0，并使垂向加速度有限衰减。

在动力学仿真软件Adams中搭建如图9所示的船舶机器人防晕平台，此处的机器人机构采用6自由度stewart构型。按照前述的设计方法，在Simulink软件中搭建控制率，并将其和Adams软件连接，进行Adams+Simulink联合仿真。

在模型的底座施加复合激励，垂向激励(y方向)为幅值100mm，频率3Hz的正弦波；横摇角激励(α方向)为幅值10°，频率0.5Hz的正弦波；纵摇角激励(γ方向)为幅值8°，频率0.8Hz的正弦波。

仿真结果如图10和图11所示。图10中，上台面垂向位移在经历初始的波动后，很快进入稳态，稳态的位移变动范围为2～7mm，相比于底座-100～100mm的位移变动，可减少97.5％的振动。且通过多个仿真表明，频率越高的垂向激励衰减越明显，符合图8的伯德图。该控制方法可以对y方向的激励实现全频段衰减，特别是对于3Hz以后的中高频段几乎可以完全消除。

图11中，alpha是横摇角的曲线，gamma是纵摇角的曲线，可以看出，上台面的横摇角和纵摇角在经历初始波动后，可以很快达到稳定，稳定精度达到0.02°。在实际使用过程中，海浪的横摇角和纵摇角的周期一般为0.5s～10s，为低频段激励，本发明提出的姿态稳定方案可以达到极好的稳定效果，使得人体几乎感觉不到船舶的晃动，有效消除晕船症状。

本实施例，实现了姿态平衡和垂向减振的一体化，针对主要由船舶摇摆引起的船舶晕动症，进行姿态调整，消除船舶的横摇角和纵摇角；同时，针对船舶垂荡造成的失重感，进行垂向的加速度衰减，从而实现更好的防治晕船效果。

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

Claims (4)

1.一种主动减少晃动的力控方法，用于在载具行驶过程中减少载具内支撑件的晃动，其特征在于，包括：

获取载具行驶过程中在第一时刻的第一载具振动信息和第一支撑件振动信息；

基于所述载具和所述支撑件之间的动力学关系、所述第一载具振动信息和所述第一支撑件振动信息，确定任务空间的第一控制力；

基于所述第一控制力控制执行机构带动所述支撑件相对于所述载具运动，以在所述载具行驶过程中减少所述支撑件的晃动；

获取载具行驶过程中在第二时刻的第二载具振动信息和第二支撑件振动信息；

获取完成与所述第一控制力对应的第一执行动作后的所述执行机构的关节空间位移；

根据所述执行机构的关节空间位移，进行运动学正解得到所述载具和所述支撑件之间的相对运动信息；

基于所述第二载具振动信息、所述第二支撑件振动信息，以及所述载具和所述支撑件之间的相对运动信息，确定横摇的控制率、纵摇的控制率和垂荡的控制率；

基于所述载具和所述支撑件之间的动力学关系、所述横摇的控制率、所述纵摇的控制率和所述垂荡的控制率，确定所述任务空间的第二控制力；

根据所述任务空间的第二控制力，进行运动学逆解，得到所述执行机构在关节空间的控制力；

执行所述执行机构在关节空间的控制力对应的指令，所述执行机构带动所述支撑件相对于所述载具运动；

其中采用PID控制算法确定所述横摇的控制率和所述纵摇的控制率，采用弹簧阻尼和天棚阻尼控制算法确定所述垂荡的控制率，包括：采用下述公式计算横摇的控制率纵摇的控制率/>和垂荡的控制率/>

其中，为所述第二载具振动信息中的垂向加速度，/>为第二载具振动信息中的横摇角加速度，/>为第二载具振动信息中的纵摇角加速度，/>为所述第二支撑件振动信息中的垂向速度，α_B为所述第二支撑件振动信息中的横摇角，/>为所述第二支撑件振动信息中的横摇角速度，γ_B为所述第二支撑件振动信息中的纵摇角，/>为所述第二支撑件振动信息中的纵摇角速度，Δy为所述相对运动信息中的垂向位移，/>为所述相对运动信息中的垂向速度，k_y、c_y、s_y为垂荡控制参数，k_α、c_α为横摇控制参数，k_γ、c_γ为纵摇控制参数；

采用以下公式计算任务空间的第二控制力f_c：

其中，ΔX为所述相对运动信息中的相对位移信息，所述相对运动信息中的相对速度信息，/>为任务空间的惯性力项，/>为任务空间的柯氏力与离心力项，η(ΔX)为任务空间的重力项，/>为所述垂荡的控制率，/>为所述横摇的控制率，/>为所述纵摇的控制率。

2.根据权利要求1所述的一种主动减少晃动的力控方法，其特征在于，采用安装于所述载具的惯导单元获取所述载具的所述第一载具振动信息和所述第二载具振动信息；

采用安装于所述支撑件的惯导单元获取所述支撑件的所述第一支撑件振动信息和所述第二支撑件振动信息。

3.一种主动减少晃动的力控系统，用于在载具行驶过程中减少载具内支撑件的晃动，其特征在于，包括：

振动信息获取单元，用于获取载具行驶过程中在第一时刻的第一载具振动信息和第一支撑件振动信息；

控制力计算单元，用于基于所述载具和所述支撑件之间的动力学关系、所述第一载具振动信息和所述第一支撑件振动信息，确定任务空间的第一控制力；

执行控制单元，用于基于所述第一控制力控制执行机构带动所述支撑件相对于所述载具运动，以在所述载具行驶过程中减少所述支撑件的晃动；

所述振动信息获取单元，还用于获取载具行驶过程中在第二时刻的第二载具振动信息和第二支撑件振动信息；

信息反馈单元，用于获取完成与所述第一控制力对应的第一执行动作后的所述执行机构的关节空间位移；根据所述执行机构的关节空间位移，进行运动学正解得到所述载具和所述支撑件之间的相对运动信息；

所述控制力计算单元，还用于基于所述第二载具振动信息、所述第二支撑件振动信息，以及所述载具和所述支撑件之间的相对运动信息，确定横摇的控制率、纵摇的控制率和垂荡的控制率；基于所述载具和所述支撑件之间的动力学关系、所述横摇的控制率、所述纵摇的控制率和所述垂荡的控制率，确定所述任务空间的第二控制力；

其中采用PID控制算法确定所述横摇的控制率和所述纵摇的控制率，采用弹簧阻尼和天棚阻尼控制算法确定所述垂荡的控制率，包括：采用下述公式计算横摇的控制率纵摇的控制率/>和垂荡的控制率/>

其中，为所述第二载具振动信息中的垂向加速度，/>为第二载具振动信息中的横摇角加速度，/>为第二载具振动信息中的纵摇角加速度，/>为所述第二支撑件振动信息中的垂向速度，α_B为所述第二支撑件振动信息中的横摇角，/>为所述第二支撑件振动信息中的横摇角速度，γ_B为所述第二支撑件振动信息中的纵摇角，/>为所述第二支撑件振动信息中的纵摇角速度，Δy为所述相对运动信息中的垂向位移，/>为所述相对运动信息中的垂向速度，k_y、c_y、s_y为垂荡控制参数，k_α、c_α为横摇控制参数，k_γ、c_γ为纵摇控制参数；

采用以下公式计算任务空间的第二控制力f_c：

其中，ΔX为所述相对运动信息中的相对位移信息，所述相对运动信息中的相对速度信息，/>为任务空间的惯性力项，/>为任务空间的柯氏力与离心力项，η(ΔX)为任务空间的重力项，/>为所述垂荡的控制率，/>为所述横摇的控制率，/>为所述纵摇的控制率；

所述执行控制单元，还用于根据所述任务空间的第二控制力，进行运动学逆解，得到所述执行机构在关节空间的控制力；执行所述执行机构在关节空间的控制力对应的指令，所述执行机构带动所述支撑件相对于所述载具运动。

4.一种自平衡装置，其特征在于，包括：

底座，固定于载具；

第一惯导单元，安装于所述底座或所述载具，用于检测所述载具的振动信息；

执行机构，采用机器人构型，包括驱动器；

编码器，安装于所述执行机构，用于检测所述执行机构的关节空间位移；

支撑件，所述执行机构安装于所述底座和所述支撑件之间；

第二惯导单元，安装于所述支撑件，用于检测所述支撑件的振动信息；

控制单元，与所述第一惯导单元、所述第二惯导单元、所述编码器、所述驱动器电性连接，所述控制单元包括控制器和存储器，所述存储器用于存储计算机程序，所述控制器用于运行所述计算机程序时实现如权利要求1至2中任一项所述的主动减少晃动的力控方法。