CN114802594A - 一种升沉补偿的三自由度船载稳定平台 - Google Patents

Abstract

针对船载设备由于海浪干扰产生摇摆的问题，设计一种升沉补偿的三自由度船载稳定平台。本发明装置由减横摇外环、减纵摇内环、升沉补偿系统、PLC控制箱、双向阻尼器、多角度传感器、超声波距离传感器、波高观测系统、横纵摇伺服系统和载物平台等部分组成。减横摇外环、减纵摇内环和升沉补偿系统保持载物平台在横纵摇及升沉方向的位置恒定，采用角度传感器测量横、纵摇角信号，进而驱动横、纵摇伺服系统，抑制装置在横纵摇方向的运动，利用波高观测系统获取波高后计算补偿后期望位置，带入阻抗公式获得系统输出力，补偿升沉方向的干扰。本发明对载物平台的横、纵摇及升沉三自由度分别进行干扰补偿，本装置的稳定性强，响应速度快，控制精度高。

Description

一种升沉补偿的三自由度船载稳定平台

技术领域

本发明属于船载特辅装备，尤其涉及一种升沉补偿的三自由度船载稳定平台装置。

背景技术

船载稳定平台作为一类常见的船载装备，被广泛应用于辅助人员和货物转运、吊装作业、医疗急救方面。由于海浪干扰会对各类作业与人员安全性产生影响。因此，对于安装在甲板平面的稳定平台的横摇角，纵摇角进行抑制，对装置升沉运动进行补偿，对保持载物平台上各类作业的精确性和稳定性具有重要意义。

目前，常见的稳定平台鲜有能对升沉运动进行补偿的装置出现，且多数装置属于被动控制，其控制精度和响应速度相对较差。中国专利CN105173024A三自由度液压驱动重载稳定平台，其机械结构在高海况下适应性差，本专利具有更大的极限角度并对结构进行了建模分析，且该专利所使用的滚珠结构面对高频升沉运动会对其机械结构造成损伤。中国专利CN103760811舰载重型稳定平台在横、纵摇可进行一定程度的减摇，但其二自由度对于升沉运动无法进行补偿，会增大机械结构的承重能力，且其减摇方法属于被动减摇。

发明内容

本发明的目的在于提供一种升沉补偿的三自由度船载稳定平台，可对装置在三自由度的位置偏移进行有效抑制，解决船载设备在各类复杂海况下横摇、纵摇和升沉运动的问题。

为了实现上述目的，本发明设计了一种升沉补偿的三自由度船载稳定平台，主要由刚性固定架，刚性连杆，减横摇外环，差速连杆一，横摇伺服系统，横摇角度传感器，减纵摇内环，差速连杆二，纵摇伺服系统，纵摇角度传感器，升沉补偿系统，双向阻尼器，升沉角度传感器，超声波距离传感器，滑动筒，铰链，波高观测系统，PLC控制箱，载物平台。本发明还可以包括：

刚性固定架与减横摇外环，减横摇外环与减纵摇内环，减纵摇内环与升沉补偿系统依次通过刚性连杆，差速连杆一，差速连杆二连接。横、纵摇角度传感器采集的横、纵摇信号后，PLC控制箱控制横、纵摇伺服系统，从而驱动执行机构差速连杆进行横、纵摇自由度的运动抑制；PLC控制箱获取波高观测系统测得波高，后计算双向阻尼器需要补偿的位移量，带入阻抗模型后输出参考力，从而使得载物平面保持位置恒定。

减横、纵摇装置结构设计：

甲板平台放置刚性固定架，底端与母船甲板直接连接，弧形底座内部通过刚性连杆沿着OY方向安装减横摇外环装置；减纵摇内环装置设置在减横摇外环装置内部，其半径小于外环装置，减横摇装置和减纵摇装置之间通过差速连杆一进行连接，其方向沿着OX方向，减横摇外环内面上安装横摇角度传感器，用以测量减横摇外环的横摇角度θ_x，差速连杆一上装有横摇伺服系统，在采集横摇角θ_x后驱动执行机构差速连杆一，进行横摇自由度的运动抑制；减纵摇装置和升沉补偿系统之间通过差速连杆二进行连接，其方向沿着OY垂直于差速连杆一，减纵摇内环内面上装有纵摇角度传感器，用以测量减纵摇内环的纵摇角度θ_y，差速连杆二上安装有纵摇伺服系统，在采集纵摇角θ_y后驱动执行机构差速连杆二，进行纵摇自由度的运动抑制。

升沉补偿机构结构设计：

升沉补偿系统安装在减纵摇装置内部空间，主要由六块刚性支撑、两块装有滑动筒的竖向刚性支撑和一个双向阻尼器组成；两块安装滑动筒的刚性支撑套接在差速连杆二上，当补偿位置接近极限位置时将锁死装置，避免与减纵摇内环发生碰撞，从而保护设备安全；另外两块竖向刚性支撑之间安装双向阻尼器，可在OX方向进行运动补偿；四块刚性支撑下端通过铰链在X,Y方向与四块竖向刚性支撑连接，上端设置铰链与载物平台相连；PLC控制箱控制双向阻尼器进行补偿时，使斜向支撑绕上端铰链进行转角大小为φ_z的旋转，目的是减小斜向刚性支撑在OZ方向的位移，后升沉角度传感器测量偏转角度，PLC控制箱将实际角度与φ_z进行比较后判断误差是否在允许范围内，达到闭环控制的效果，从而保持载物平台位置恒定；

所述减摇圆环，忽略其厚度后进行建模，由于减摇圆环左右为对称结构，因此对其左半部分进行分析。首先依据转动惯量定义式，求取球壳转动惯量：

J＝∫r²dm (1)

针对半窄圆环的转动惯量，半窄圆环可视为球壳的一部分，J为球壳转动惯量，J′为半圆环转动惯量，r为圆环半径，m为球壳质量，R为球壳半径，c为减摇环宽度，由几何关系和窄圆环和球壳关系，引入比例因子

得到：

对于半圆环的质心位置，首先建立笛卡尔坐标系，L′为半圆弧弧长，质心坐标为r₀(x,y,z)，依据对称性y＝0，z＝0。因此由质心公式可知：

针对减横、纵环进行拉格朗日建模，已知系统的拉格朗日算子为：

L＝E_K-E_G (4)

L为拉格朗日函数，E_K为系统动能，E_G为系统势能。

设初始时刻动能为0，则运动后减横摇外环的动能为：

令甲板平面为重力势能0参考面，E_Kx为减横摇半圆环动能，

为横摇角速度，J_x为减横摇圆环转动惯量，m_x为减横摇左半圆环质量，R_x为减横摇环半径，c_x为减横摇圆环宽度，则减横摇左半圆环势能为：

式中，E_Gx为减横摇左半圆环重力势能，θ_x为横摇角，h为超声波距离传感器测得转轴与甲板的距离，g为重力加速度。

则减横摇左半圆环进行拉格朗日动力学模型为：

式中，

为横摇角加速度，

为横摇转动力矩。

同理，减纵摇前半圆环的拉格朗日动力学模型为：

式中，m_y为减纵摇前半圆环质量，R_y为减纵摇前半圆环半径，c_y为减纵摇圆环宽度，θ_y为纵摇角，

为纵摇角加速度，

为纵摇转动力矩。

对于升沉补偿机构，l为刚性支撑板长，支撑结构的转动惯量为

其质心在支撑结构的几何中心。

设系统初始动能为0，升沉补偿系统初始角度为φ₀，在补偿后系统角度变为φ_z。并且载物平台在升沉自由度保持位置恒定，则运动后升沉补偿系统的动能为：

式中，E_kz为升沉补偿系统动能，

为升沉补偿系统转动角速度，m_z为刚性支撑质量，系统在末位置具有的重力势能为：

式中，E_Gz为升沉补偿系统重力势能。

升沉方向拉格朗日运动学模型为：

式中，

为升沉补偿系统转动力矩，

为升沉补偿系统转动角加速度。

对基于阻抗控制的双向阻尼器，阻抗代表的是双向阻尼器末端接触力与期望轨迹与实际轨迹之间差之间的关系；

M_d，B_d，K_d分别代表期望阻抗模型的惯性矩阵、刚度矩阵以及阻尼矩阵；

x分别代表双向阻尼器的末端加速度、末端速度以及末端位置，x_r为末端期望位置，f_r，f_e分别为双向阻尼器的关节参考力和末端接触力，可进行力的跟踪控制；

对于本装置，其阻抗模型可表示为：

式中，

D为当前转动力臂大小，H为波高观测系统所测得波高。

取静止时双向阻尼器几何中心为坐标原点，ox轴为原点指向船首方向，oy轴为原点指向船侧建立0#坐标系，设假设母船横摇角为θ_x，纵摇角为θ_y，升沉补偿系统偏转角为φ_z，即装置坐标沿船体坐标系旋转θ_x，沿OY轴旋转θ_y，升沉方向位移变化为l(sinφ_z-sinφ₀)。则稳定平台几何中心相对于0#坐标系的坐标变换矩阵为T_P，减横、纵摇圆环几何中心以及升沉补偿系统中心相对于0#坐标系的坐标变换矩阵分别为T_x，T_y和T_z。因此运动后装置坐标系在船体横摇、纵摇和升沉作用影响下相对于0#坐标系的坐标变换矩阵为：

T_P＝T_zT_yT_x＝Trans(0,0，l(sinφ_z-sinφ₀))Rot(y,θ_y)Rot(x,θ_x) (13)

根据上述坐标变换，可求出装置位置对于母船坐标系的变换矩阵。

式中，

为升沉系统期望的偏转夹角。

根据上述坐标变换，可求出每个关节在横摇角θ_x和纵摇角θ_y分别需要补偿的角度的表达式，即为三自由度稳定平台的逆运动学解。

本发明具有如下有益效果：

(1)本发明所选用的双减摇环与升沉补偿系统组合的方式具有横摇、纵摇以及升沉三个自由度，能够使得平台在母船受到干扰产生摇摆运动的情况下依然保持相对稳定，该装置极大提高工作效率和稳定平台上作业的安全性。

(2)本发明设计了一种升沉补偿的三自由度稳定平台和建模方法，对三自由度建立的模型普适性强，本发明装置具有稳定性高，响应速度快，精度高的特点。

(3)本发明设计的升沉补偿系统，通过设置在船体侧面的波高观测系统获取波高，后计算双向阻尼器需要补偿的位移量，带入阻抗模型后输出参考力，在海浪干扰作用在母船之时对双向阻尼器进行主动控制，从而使得载物平面在升沉方向位置恒定。

附图说明

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

图1为三自由度稳定平台模型示意图；

图2为三自由度稳定平台模型正视图；

图3为三自由度稳定平台模型左视图；

图4为三自由度稳定平台模型俯视图；

图5为稳定平台工作模式流程图；

图6为减摇圆环建模分析示意图；

图7为减摇圆环位置变化示意图；

图8为升沉补偿位置变化示意图。

具体实施方式

下面结合附图对本发明进行详细描述：

结合图1～4，本发明的稳定平台由刚性固定架1-1，刚性连杆1-2，减横摇外环2-1，差速连杆一2-2，横摇伺服系统2-3，横摇角度传感器2-4，减纵摇内环3-1，差速连杆二3-2，纵摇伺服系统3-3，纵摇角度传感器3-4，升沉补偿系统4-1，双向阻尼器4-2，升沉角度传感器4-3，超声波距离传感器4-4，滑动筒4-5，铰链4-6，波高观测系统4-7，PLC控制箱5，载物平台6部分组成。

刚性固定架1-1与减横摇外环2-1，减横摇外环2-1与减纵摇内环3-1，减纵摇内环3-1与升沉补偿系统4-1依次通过刚性固连杆1-2，差速连杆一2-2，差速连杆二3-2连接。PLC控制箱5获取横、纵摇角度传感器2-4，3-4采集的横、纵摇信号后控制横、纵摇伺服系统2-3、3-3驱动执行机构差速连杆进行横、纵摇自由度的运动抑制；PLC控制箱5获取波高观测系统4-7测得波高，后计算双向阻尼器4-2需要补偿的位移量，带入阻抗模型后输出参考力，从而使得载物平面保持位置恒定。

减横、纵摇装置结构设计：

刚性固定架1-1包括两对称部分，主要是弧形底座和可装刚性连杆1-2的通孔构成，基座安装在母船船体上，基座上部留有沿着OY方向的通孔可与减横摇外环2-1通过刚性连杆1-2固连，且其通孔处于同一水平线。

所述减横摇外环2-1为圆环结构，其沿着OX直径的对称位置设有通孔，与刚性连杆1-2连接方向相垂直，减纵摇内环3-1为圆环结构，其沿着OY直径的对称位置设有通孔，与差速连杆一2-2连接方向相垂直。

甲板平台放置刚性固定架1-1，底端与母船甲板直接连接，弧形底座内部通过刚性连杆1-2沿着OY方向安装减横摇外环2-1装置；减纵摇内环3-1装置设置在减横摇外环2-1装置内部，其半径小于外环装置，减横摇装置和减纵摇装置之间通过差速连杆一2-2进行连接，其方向沿着OX方向，减横摇外环2-1内面上安装横摇角度传感器2-4，用以测量减横摇外环2-1的横摇角度θ_x，差速连杆一2-2上装有横摇伺服系统2-3，在采集横摇角θ_x后驱动执行机构差速连杆一2-2，进行横摇自由度的运动抑制；减纵摇装置和升沉补偿系统4-1之间通过差速连杆二3-2进行连接，其方向沿着OY垂直于差速连杆一2-2，减纵摇内环3-1内面上装有纵摇角度传感器3-4，用以测量减纵摇内环的纵摇角度θ_y，差速连杆二3-2上安装有纵摇伺服系统，在采集纵摇角θ_y后驱动执行机构差速连杆二3-2，进行纵摇自由度的运动抑制。

升沉补偿机构结构设计：

升沉补偿系统4-1为爪型结构，安装在减纵摇装置内部空间，主要由六块刚性支撑、两块装有滑动筒4-5的竖向刚性支撑和一个双向阻尼器4-2组成；两块安装滑动筒4-5的刚性支撑套接在差速连杆二3-2上，当补偿位置接近极限位置时将锁死装置，避免与减纵摇内环3-1发生碰撞，从而保护设备安全；另外两块竖向刚性支撑之间安装双向阻尼器4-2，可在OX方向进行运动补偿；四块刚性支撑下端通过铰链4-6在X,Y方向与四块竖向刚性支撑连接，上端设置铰链与载物平台6相连；PLC控制箱5控制双向阻尼器4-2进行补偿时，使斜向支撑绕上端铰链进行转角大小为φ_z的旋转，目的是减小斜向刚性支撑在OZ方向的位移，后升沉角度传感器4-3测量偏转角度，PLC控制箱5将实际角度与φ_z进行比较后判断误差是否在允许范围内，达到闭环控制的效果，从而保持载物平台6位置恒定；

减摇圆环建模分析：

对于减摇圆环，忽略其厚度后进行建模，由于减摇圆环左右为对称结构，因此对其左半部分进行分析。首先依据转动惯量定义式

J＝∫r²dm (1)

针对半窄圆环的转动惯量，半窄圆环可视为球壳的一部分，对于球壳的转动惯量，J为球壳转动惯量，J′为半圆环转动惯量，r为圆环半径，m为球壳质量，R为球壳半径，a为相邻两圆环的距离，θ为圆环对应圆心角，c为减摇环宽度；

由几何关系得r＝Rsinθ，a＝rdθ，则：

由窄圆环和球壳关系，引入比例因子

得到：

对于半圆环的质心位置，首先建立笛卡尔坐标系，L′为半圆弧弧长，质心坐标为r₀(x,y,z)，依据对称性y＝0，z＝0。

因此由质心公式可知：

减横、纵环进行拉格朗日建模分析：

已知系统的拉格朗日算子为：

L＝E_K-E_G (6)

对系统横摇，纵摇，升沉方向进行拉格朗日建模

L为拉格朗日函数，E_K为系统动能，E_G为系统势能，

为横摇转动力矩，

为纵摇转动力矩，

为升沉补偿系统4-1转动力矩，θ_x为横摇角，

为横摇角速度，θ_y为纵摇角，

为纵摇角速度，φ_z为升沉补偿系统转动角，

为升沉补偿系统转动角速度。

设初始时刻动能为0，则运动后减横摇左半圆环的动能为：

令甲板平面为重力势能0参考面，E_Kx为减横摇半圆环动能，

为横摇角速度，J_x为减横摇圆环转动惯量，m_x为减横摇左半圆环质量，R_x为减横摇左半圆环半径，c_x为减横摇圆环宽度，则减横摇左半圆环势能为：

式中，E_Gx为减横摇左半圆环重力势能，θ_x为横摇角，h为超声波距离传感器4-4测得转轴与甲板的距离，g为重力加速度。

则左半横摇环进行拉格朗日动力学模型为：

同理，减纵摇前半圆环的拉格朗日动力学模型为：

式中，m_y为减纵摇半圆环质量，R_y为减纵摇半圆环半径，c_y为减纵摇圆环宽度，θ_y为纵摇角，

为纵摇角加速度。

升沉补偿机构建模分析：

对于升沉补偿机构，l为刚性支撑板长，支撑结构的转动惯量为

其质心在支撑结构的几何中心。

设系统初始动能为0，升沉补偿系统4-1初始角度为φ₀，在补偿后系统角度变为φ_z。并且载物平台6在升沉自由度保持位置恒定，则运动后升沉补偿系统4-1的动能为：

式中，E_kz为升沉补偿系统动能，

为升沉补偿系统4-1转动角速度，m_z为刚性支撑质量，系统在末位置具有的重力势能为：

式中，E_Gz为升沉补偿系统重力势能；

升沉方向拉格朗日运动学模型为：

式中，

为升沉方向系统的拉格朗日运动学模型，

为升沉补偿系统转动角加速度；

升沉补偿策略分析：

对基于阻抗控制的双向阻尼器4-2，阻抗代表的是双向阻尼器4-2末端接触力与期望轨迹与实际轨迹之间差之间的关系，其数学表达式可以写为：

其中，M_d，B_d，K_d分别代表期望阻抗模型的惯性矩阵、刚度矩阵和阻尼矩阵。

x分别代表双向阻尼器4-2的末端加速度、末端速度以及末端位置，x_r为末端期望位置，f_r，f_e分别为双向阻尼器4-2的关节参考力和末端接触力，可进行力的跟踪控制。

对于本装置，其阻抗模型可表示为：

式中，

D为当前转动的力臂大小，H为波高观测系统4-7所测得波高。

运动学建模分析：

取静止时双向阻尼器4-2几何中心为坐标原点，OX轴为原点指向船首方向，OY轴为原点指向船侧建立0#坐标系，设假设母船横摇角为θ_x，纵摇角为θ_y，升沉补偿系统4-1偏转角为φ_z，即装置坐标沿船体坐标系旋转θ_x，沿OY轴旋转θ_y，升沉方向位移变化为l(sinφ_z-sinφ₀)。则减横摇装置几何中心相对于0#坐标系的坐标变换矩阵为T_P。减横、纵摇圆环几何中心以及升沉补偿系统中心相对于0#坐标系的坐标变换矩阵分别为T_x，T_y和T_z。因此运动后装置坐标系在船体横摇、纵摇和升沉作用影响下相对于0#坐标系的坐标变换矩阵为：

T_P＝T_zT_yT_x＝Trans(0,0，l(sinφ_z-sinφ₀))Rot(y,θ_y)Rot(x,θ_x) (17)

根据上述坐标变换，可求出装置位置对于母船坐标系的变换矩阵。

式中，

为升沉系统期望的偏转夹角。

根据上述坐标变换，可求出每个关节在横摇角θ_x和纵摇角θ_y分别需要补偿的角度的表达式，即为三自由度稳定平台的逆运动学解。

以上所述具体实施方案，对本发明的发明目的、技术方案和有益效果进行了进一步说明，以上实施例仅用于说明本发明的技术方案，而非对本发明创造保护范围的限制，本领域的普通技术人员应当理解，凡在本发明的技术方案进行修改、等同替换，均包含在本发明的保护范围内。

Claims (1)

1.一种升沉补偿的三自由度船载稳定平台，包括：刚性固定架，刚性连杆，减横摇外环，差速连杆一，横摇伺服系统，横摇角度传感器，减纵摇内环，差速连杆二，纵摇伺服系统，纵摇角度传感器，升沉补偿系统，双向阻尼器，升沉角度传感器，超声波距离传感器，滑动筒，铰链，波高观测系统，PLC控制箱，载物平台；刚性固定架与减横摇外环，减横摇外环与减纵摇内环，减纵摇内环与升沉补偿系统依次通过刚性连杆，差速连杆一，差速连杆二连接；横、纵摇角度传感器采集的横、纵摇信号后，PLC控制箱控制横、纵摇伺服系统，从而驱动执行机构差速连杆进行横、纵摇自由度的运动抑制；PLC控制箱获取波高观测系统测得波高，后计算双向阻尼器需要补偿的位移量，带入阻抗模型后输出参考力，从而使得载物平台保持位置恒定；本发明为一种升沉补偿的三自由度船载稳定平台，其特征是：

(1)甲板平台放置刚性固定架，底端与母船甲板直接连接，弧形底座内部通过刚性连杆沿着OY方向安装减横摇外环装置；减纵摇内环装置设置在减横摇外环装置内部，二者之间通过差速连杆一进行连接，其方向沿着OX方向，减横摇外环内面上安装横摇角度传感器，用以测量减横摇外环的横摇角度θ_x，差速连杆一上装有横摇伺服系统，在采集横摇角θ_x后驱动执行机构差速连杆一，进行横摇自由度的运动抑制；减纵摇装置和升沉补偿系统之间通过差速连杆二进行连接，其方向沿着OY垂直于差速连杆一，减纵摇内环内面上装有纵摇角度传感器，用以测量减纵摇内环的纵摇角度θ_y，差速连杆二上安装有纵摇伺服系统，在采集纵摇角θ_y后驱动执行机构差速连杆二，进行纵摇自由度的运动抑制；

(2)升沉补偿系统安装在减纵摇装置内部空间，主要由六块刚性支撑、两块装有滑动筒的竖向刚性支撑和一个双向阻尼器组成；两块安装滑动筒的刚性支撑套接在差速连杆二上，当补偿位置接近极限位置时将锁死装置，避免与减纵摇内环发生碰撞，从而保护设备安全；另外两块竖向刚性支撑之间安装双向阻尼器，可在OX方向进行运动补偿；四块刚性支撑下端通过铰链在X,Y方向与四块竖向刚性支撑连接，上端与载物平台相连；PLC控制箱控制双向阻尼器进行补偿时，使斜向支撑绕上端铰链进行转角大小为φ_z的旋转，目的是减小斜向刚性支撑在OZ方向的位移，后升沉角度传感器测量偏转角度，PLC控制箱将实际角度与φ_z进行比较后判断误差是否在允许范围内，达到闭环控制的效果，从而保持载物平台位置恒定；

(3)对于减摇圆环，忽略其厚度后进行建模，由于减摇圆环左右为对称结构，因此对其左半部分进行分析，转动惯量定义式为：

J＝∫r²dm (1)

针对半窄圆环的转动惯量，半窄圆环可视为球壳的一部分，J为球壳转动惯量，J′为半圆环转动惯量，r为圆环半径，m为球壳质量，R为球壳半径，c为减摇环宽度，由几何关系和窄圆环和球壳关系，引入比例因子

得到：

对于半圆环质心位置，建立笛卡尔坐标系，L′为半圆弧弧长，质心坐标为r₀(x,y,z)，依据对称性y＝0，z＝0，因此由质心公式可知：

(4)对减横、纵环进行拉格朗日建模，系统的拉格朗日算子为：

L＝E_K-E_G (4)

式中，L为拉格朗日函数，E_K为系统动能，E_G为系统势能；

设初始时刻动能为0，则运动后减横摇外环的动能为：

令甲板平面为重力势能0参考面，E_Kx为减横摇半圆环动能，

为横摇角速度，J_x为减横摇圆环转动惯量，m_x为减横摇左半圆环质量，R_x为减横摇环半径，c_x为减横摇圆环宽度，则减横摇左半圆环势能为：

式中，E_Gx为减横摇左半圆环重力势能，θ_x为横摇角，h为超声波距离传感器测得转轴与甲板的距离，g为重力加速度；

则减横摇左半圆环进行拉格朗日动力学模型为：

式中，

为横摇角加速度，

为横摇转动力矩；

同理，减纵摇半环的拉格朗日动力学模型为：

式中，m_y为减纵摇半圆环质量，R_y为减纵摇半圆环半径，c_y为减纵摇圆环宽度，θ_y为纵摇角，

为纵摇角加速度，

为纵摇转动力矩；

(5)对于升沉补偿系统，l为刚性支撑板长，支撑结构的转动惯量为

其质心在支撑结构的几何中心，设系统初始动能为0，升沉补偿系统初始角度为φ₀，在补偿后系统角度变为φ_z，并且载物平台在升沉自由度保持位置恒定，则运动后升沉补偿系统的动能为：

式中，E_kz为升沉补偿系统动能，

为升沉补偿系统转动角速度，m_z为刚性支撑质量，系统在末位置具有的重力势能为：

式中，E_Gz为升沉补偿系统重力势能；

升沉方向拉格朗日运动学模型为：

式中，

为升沉补偿系统转动力矩，

为升沉补偿系统转动角加速度；

(6)对基于阻抗控制的双向阻尼器，阻抗代表的是双向阻尼器末端接触力与期望轨迹与实际轨迹之间差之间的关系；

M_d，B_d，K_d分别代表期望阻抗模型的惯性矩阵、刚度矩阵以及阻尼矩阵；

x分别代表双向阻尼器的末端加速度、末端速度以及末端位置，x_r为末端期望位置，f_r，f_e分别为双向阻尼器的关节参考力和末端接触力，可进行力的跟踪控制；

对于本装置，其阻抗模型可表示为：

式中，

D为当前转动力臂大小，H为波高观测系统所测得波高；

(7)取静止时双向阻尼器几何中心为坐标原点，ox轴为原点指向船首方向，oy轴为原点指向船侧建立0#坐标系，设假设母船横摇角为θ_x，纵摇角为θ_y，升沉补偿系统偏转角为φ_z，即装置坐标沿船体坐标系OX轴旋转θ_x，沿OY轴旋转θ_y，升沉方向位移变化为l(sinφ_z-sinφ₀)；则稳定平台中心相对于0#坐标系的坐标变换矩阵为T_P，减横、纵摇圆环几何中心以及升沉补偿系统中心相对于0#坐标系的坐标变换矩阵分别为T_x，T_y和T_z；因此运动后装置坐标系在船体横摇、纵摇和升沉作用影响下相对于0#坐标系的坐标变换矩阵为：

T_P＝T_zT_yT_x＝Trans(0,0，l(sinφ_z-sinφ₀))Rot(y,θ_y)Rot(x,θ_x) (13)

根据上述坐标变换，可求出装置位置对于母船坐标系的变换矩阵：

式中，

为升沉系统期望的偏转夹角；

根据上述坐标变换，可求出每个关节在横摇角θ_x和纵摇角θ_y分别需要补偿的角度的表达式，即为三自由度稳定平台的逆运动学解。

Images