CN112613167B - 一种基于绳索操控的两体动力学建模方法 - Google Patents

Abstract

本发明实施例公开一种基于绳索操纵的两体动力学建模方法，包括：S1、计算绳索长度及绳长变化率；S3、求取绳索姿态及绳索姿态角速度；S5、约束分离广义坐标，得到缆绳收放的速度；S7、根据加速度约束方程建立动力学方程；S9、在所述加速度约束方程中加入修正项，用绳长的变化量和绳长的导数进行反馈，得到修正后的动力学方程。

Description

一种基于绳索操控的两体动力学建模方法

技术领域

本发明涉及力学领域。更具体地，涉及一种基于绳索操控的两体动力学建模方法。

背景技术

近二十年，基于柔性绳索的多刚体动力学研究受到广泛关注，典型项目有空间绳索、水下电缆、伞舱回收系统、动力翼伞等。研究内容涉及多体动力学建模以及绳索动力学，一般从矢量力学和分析力学两个方向建模。矢量力学的理论基础是牛顿力学，通过分析对象的平衡力系以及速度、加速度之间关系进行分析，此方法需要考虑系统内部的约束力；而分析力学的理论基础是虚功原理，利用分析对象所受外力做的功和对象的能与速度、加速度之间关系进行分析，此方法着眼于整个系统，避免了约束反力。

传统研究都是根据被控对象特点采取一些假设处理，如吊点方式的简化、两体自由度的简化等，而且没有考虑绳索长度变化影响。

发明内容

有鉴于此，本发明第一个实施例提供一种基于绳索操纵的两体动力学建模方法，包括：

S1、计算绳索长度及绳长变化率；

S3、求取绳索姿态及绳索姿态角速度；

S5、约束分离广义坐标，得到缆绳收放的速度；

S7、根据加速度约束方程建立动力学方程；

S9、在所述加速度约束方程中加入修正项，用绳长的变化量和绳长的导数进行反馈，得到修正后的动力学方程。

在一个具体实施例中，所述S1包括：

S11、获取初始时气囊体系和吊舱体系下的吊点坐标；

S13、求解发射系下气囊和吊舱上的吊点坐标；

S15、根据发射系下的吊点坐标求解得到绳索长度，利用相邻两个时刻的绳索长度差分，从而计算得到绳长变化率：

式中，l_{c_p}(k)为当前时刻的绳长，l_{c_p}(k-1)为上一时刻的绳长，ΔT为操作时长。

在一个具体实施例中，所述S3包括：

S31、根据绳索在气囊体系下的坐标，求解得到绳索体系相对气囊的方位角和仰角：

θ_T＝arctan(-l_{c_p_x}/l_{c_p_y})

γ_T＝arctan(l_{c_p_z}/(l_{c_p_y}cos(θ_T)-l_{c_p_x}sin(θ_T)))

式中，l_{c_p_x}，l_{c_p_y}，l_{c_p_z}为绳索矢量在气囊体系下的坐标分量，θ_T，γ_T为绳索相对气囊的方位角和仰角；

S33、根据所述方位角和仰角得到绳索体系相对气囊体系的坐标转换矩阵A_{T_c}：

S35、并利用相邻两个时刻的方位角和仰角差分，计算得到方位角和仰角的角速度：

S37、进一步得到绳索体系相对气囊体系的角速度

在一个具体实施例中，所述S5包括：

缆绳收放的速度为约束引入的广义坐标，增广后的广义坐标如下：

式中，为气囊质心相对发射系原点在气囊体系下的速度，/>为气囊在气囊体系下的角速度，/>为吊舱质心相对发射系原点在吊舱体系下的速度，/>为吊舱在吊舱体系下的角速度，Δv_T1,Δv_T2,Δv_T3,Δv_T4为连接气囊吊舱的四根绳索收放的速度,记为：

计算缆绳收放的速度如下

式中，为气囊上吊点相对发射系原点在气囊体系下的速度，/>为气囊吊点相对吊舱吊点的位置矢径，S()为反对称算子，对/>有：

式中A_{T_p}表示绳索体系到吊舱体系的姿态转换矩阵：

表示为矩阵形式如下：

在一个具体实施例中，所述S7包括：

利用速度约束方程，求导得到加速度约束方程；

同时得到作用在两体系统上的约束反力；

根据加速度约束方程和两体动力学方程联立，得到系统的动力学方程。

在一个具体实施例中，所述S9包括：

在加速度约束方程中加入修正量，用绳长的变化量和绳长的导数进行反馈对约束方程进行修正，得到修正后的动力学方程。

在一个具体实施例中，其特征在于，

所述气囊体坐标系定义为以气囊质心为原点，x轴在囊体纵对称面内指向前进方向为正，y轴在囊体纵对称面内竖直向上为正，z轴定义符合右手定则；

所述吊舱体坐标系定义为以吊舱质心为原点，x轴在吊舱纵对称面内指向前进方向为正，y轴在吊舱纵对称面内竖直向上为正，z轴定义符合右手定则；

发射系到气囊体系的坐标转换矩阵为：

发射系到吊舱体系的坐标转换矩阵：

式中，分别为气囊的俯仰角，偏航角，滚转角；/>分别为吊舱的俯仰角，偏航角，滚转角。

本发明的第三个实施例提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如第一个实施例所述的方法。

本发明的第四个实施例提供一种计算设备，包括处理器，其特征在于，所述处理器执行程序时实现如第一个实施例所述的方法。

本发明的有益效果如下：

本发明实施例提供的一种基于绳索操控的两体动力学建模方法，该方法充分考虑了两体六自由度模型、吊点模型、绳索受力模型，能够表明气囊和吊舱两体运动过程中的位置变化、姿态变化和绳索受力变化，有利于指导两体控制。

附图说明

下面结合附图对本发明的具体实施方式作进一步详细的说明。

图1示出根据本发明的一个实施例的一种基于绳索操控的两体动力学建模方法流程图。

图2示出根据本发明的一个实施例的计算机设备的结构示意图。

具体实施方式

为了更清楚地说明本发明，下面结合优选实施例和附图对本发明做进一步的说明。附图中相似的部件以相同的附图标记进行表示。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，不应以此限制本发明的保护范围。

实施例1

一种基于绳索操控的两体动力学建模方法：

坐标系及转换矩阵的定义：

两体系统坐标系的定义，包括：发射坐标系、气囊体坐标系、吊舱体坐标系、绳索体坐标系。

发射系到气囊体系的坐标转换矩阵的功能为：描述囊体在发射系中的姿态，实现发射系到气囊体系的坐标转换。

发射系到吊舱体系的坐标转换矩阵的功能为：描述吊舱在发射系中的姿态，实现发射系到吊舱体系的坐标转换。

绳索体系到气囊体系的坐标转换矩阵的功能为：描述绳索体系相对气囊体系的相对姿态，实现绳索体系到气囊体系的坐标转换。

发射坐标系定义：发射点为发射坐标系原点，x轴沿当地水平面指向正东方向，y轴在包含x轴的铅垂面内垂直于x轴向上，z轴定义符合右手定则。

气囊体坐标系定义：以气囊质心为原点，x轴在囊体纵对称面内指向前进方向为正。y轴在囊体纵对称面内竖直向上为正，z轴定义符合右手定则。

吊舱体坐标系定义：以吊舱质心为原点，x轴在吊舱纵对称面内指向前进方向为正。y轴在吊舱纵对称面内竖直向上为正，z轴定义符合右手定则。

绳索体坐标系：以绳索和气囊的连接点为坐标原点，y轴方向沿绳索向上，z轴在发射坐标系的yoz平面内，x轴定义符合右手定则。

转换矩阵定义如下，发射系到气囊体系的坐标转换矩阵：

发射系到吊舱体系的坐标转换矩阵：

式中，分别为气囊的俯仰角，偏航角，滚转角。/>分别为吊舱的俯仰角，偏航角，滚转角。

发射系到气囊体系的坐标转换矩阵的功能为：描述囊体在发射系中的姿态，实现发射系到气囊体系的坐标转换。

发射系到吊舱体系的坐标转换矩阵的功能为：描述吊舱在发射系中的姿态，实现发射系到吊舱体系的坐标转换。

S1、绳索长度及绳长变化率的求取

根据初始时气囊体系和吊舱体系下的吊点坐标，利用第一步中得到的坐标转换矩阵求解得到发射系下气囊和吊舱上的吊点坐标，进一步根据发射系下的吊点坐标求解得到绳索长度，利用相邻两个时刻的绳索长度差分，从而计算得到绳长变化率：

式中，l_{c_p}(k)为当前时刻的绳长，l_{c_p}(k-1)为上一时刻的绳长，ΔT为仿真时长。

S3、绳索姿态及绳索姿态角速度的求取

利用绳索在气囊体系下的坐标，求解得到绳索体系相对气囊的方位角和仰角：

θ_T＝arctan(-l_{c_p_x}/l_{c_p_y})

γ_T＝arctan(l_{c_p_z}/(l_{c_p_y}cos(θ_T)-l_{c_p_x}sin(θ_T)))

式中，l_{c_p_x}，l_{c_p_y}，l_{c_p_z}为绳索矢量在气囊体系下的坐标分量，θ_T，γ_T为绳索相对气囊的方位角和仰角。

进一步可根据方位角γ_T和仰角θ_T得到绳索体系相对气囊体系的坐标转换矩阵A_{T_c}：

并利用相邻两个时刻的方位角和仰角差分，计算得到方位角和仰角的角速度：

进一步得到绳索体系相对气囊体系的角速度

S5、约束分离广义坐标

对于两体绳系动力学系统，系统的自由度为12，缆绳收放的速度为约束引入的广义坐标，增广后的系统的广义坐标如下：

式中，为气囊质心相对发射系原点在气囊体系下的速度，/>为气囊在气囊体系下的角速度，/>为吊舱质心相对发射系原点在吊舱体系下的速度，/>为吊舱在吊舱体系下的角速度，Δv_T1,Δv_T2,Δv_T3,Δv_T4为连接气囊吊舱的四根绳索收放的速度,记为：

利用气囊和吊舱的速度，角速度，可以计算得到缆绳收放的速度如下

式中，/>为气囊上吊点相对发射系原点在气囊体系下的速度，/>为气囊吊点相对吊舱吊点的位置矢径。S为反对称算子，/>有：

式中A_{T_p}表示绳索体系到吊舱体系的姿态转换矩阵：

进一步表示为矩阵形式如下：

S7、加速度约束方程下的动力学方程建立

利用速度约束方程：

Bv+b＝0

求导得到加速度约束方程：

同时得到作用在两体系统上的约束反力，式中b为绳索收放的速度，c为绳索收放的加速度。这样根据加速度约束方程和两体动力学方程联立，则可得到系统的动力学方程。

式中，M为系统的广义质量，h为系统所受的外力和外力矩：

m_c是气囊质量乘以3阶单位阵得到，I_c是气囊转动惯量，m_d是吊舱质量乘以三阶单位阵得到，I_d是吊舱转动惯量。

S9、违约稳定修正

为了防止因为数值计算误差导致的模型发散，在加速度约束方程中加入修正项，用绳长的变化量和绳长的导数进行反馈：

其中α,β为违约稳定系数，l和i是S1中求解得到的绳长和绳长变化率，a，b是由控制器给出的绳长和绳长变化率。这样得到修正后的动力学方程如下：

至此，完成了基于绳索操控的两体动力学建模方法。

本发明实施例提供的一种基于绳索操控的两体动力学建模方法，该方法充分考虑了两体六自由度模型、吊点模型、绳索受力模型，能够表明气囊和吊舱两体运动过程中的位置变化、姿态变化和绳索受力变化，有利于指导两体控制。

实施例2

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现实施例1所述的方法。

在实际应用中，所述计算机可读存储介质可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、电线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

实施例3

如图2所示，本发明的另一个实施例提供的一种计算机设备的结构示意图。图2显示的计算机设备12仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。

如图2所示，计算机设备12以通用计算设备的形式表现。计算机设备12的组件可以包括但不限于：一个或者多个处理器或者处理单元16，系统存储器28，连接不同系统组件(包括系统存储器28和处理单元16)的总线18。

总线18表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，外围总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构(ISA)总线，微通道体系结构(MAC)总线，增强型ISA总线、视频电子标准协会(VESA)局域总线以及外围组件互连(PCI)总线。

计算机设备12典型地包括多种计算机系统可读介质。这些介质可以是任何能够被计算机设备12访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。

系统存储器28可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(RAM)30和/或高速缓存存储器32。计算机设备12可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例，存储系统34可以用于读写不可移动的、非易失性磁介质(图2未显示，通常称为“硬盘驱动器”)。尽管图2中未示出，可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器，以及对可移动非易失性光盘(例如CD-ROM，DVD-ROM或者其它光介质)读写的光盘驱动器。在这些情况下，每个驱动器可以通过一个或者多个数据介质接口与总线18相连。存储器28可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明各实施例的功能。

具有一组(至少一个)程序模块42的程序/实用工具40，可以存储在例如存储器28中，这样的程序模块42包括但不限于操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块42通常执行本发明所描述的实施例中的功能和/或方法。

计算机设备12也可以与一个或多个外部设备14(例如键盘、指向设备、显示器24等)通信，还可与一个或者多个使得用户能与该计算机设备12交互的设备通信，和/或与使得该计算机设备12能与一个或多个其它计算设备进行通信的任何设备(例如网卡，调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口22进行。并且，计算机设备12还可以通过网络适配器20与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。如图2所示，网络适配器20通过总线18与计算机设备12的其它模块通信。应当明白，尽管图2中未示出，可以结合计算机设备12使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。

处理器单元16通过运行存储在系统存储器28中的程序，从而执行各种功能应用以及数据处理，例如实现本发明实施例1所提供方法。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (6)

1.一种基于绳索操纵的两体动力学建模方法，其特征在于，包括：

S1、计算绳索长度及绳长变化率；

S3、求取绳索姿态及绳索姿态角速度；

S5、约束分离广义坐标，得到缆绳收放的速度；

S7、根据加速度约束方程建立动力学方程；

S9、在所述加速度约束方程中加入修正项，用绳长的变化量和绳长的导数进行反馈，得到修正后的动力学方程；

所述S1包括：

S11、获取初始时气囊体系和吊舱体系下的吊点坐标；

S13、求解发射系下气囊和吊舱上的吊点坐标；

S15、根据发射系下的吊点坐标求解得到绳索长度，利用相邻两个时刻的绳索长度差分，从而计算得到绳长变化率：

式中，l_{c_p}(k)为当前时刻的绳长，l_{c_p}(k-1)为上一时刻的绳长，ΔT为操作时长；

所述S3包括：

S31、根据绳索在气囊体系下的坐标，求解得到绳索体系相对气囊的方位角和仰角：

θ_T＝arctan(-l_{c_p_x}/l_{c_p_y})

γ_T＝arctan(l_{c_p_z}/(l_{c_p_y}cos(θ_T)-l_{c_p_x}sin(θ_T))

式中，l_{c_p_x}，l_{c_p_y}，l_{c_p_z}为绳索矢量在气囊体系下的坐标分量，θ_T，γ_T为绳索相对气囊的方位角和仰角；

S33、根据所述方位角和仰角得到绳索体系相对气囊体系的坐标转换矩阵A_{T_c}：

S35、并利用相邻两个时刻的方位角和仰角差分，计算得到方位角和仰角的角速度：

S37、进一步得到绳索体系相对气囊体系的角速度

所述S5包括：

缆绳收放的速度为约束引入的广义坐标，增广后的广义坐标如下：

式中，为气囊质心相对发射系原点在气囊体系下的速度，/>为气囊在气囊体系下的角速度，/>为吊舱质心相对发射系原点在吊舱体系下的速度，/>为吊舱在吊舱体系下的角速度，Δv_T1,Δv_T2,Δv_T3,Δv_T4为连接气囊吊舱的四根绳索收放的速度,记为：

计算缆绳收放的速度如下

式中，为气囊上吊点相对发射系原点在气囊体系下的速度，/>为气囊吊点相对吊舱吊点的位置矢径，S()为反对称算子，对/>有：

式中A_{T_p}表示绳索体系到吊舱体系的姿态转换矩阵：

表示为矩阵形式如下：

2.根据权利要求1所述的方法，其特征在于，所述S7包括：

利用速度约束方程，求导得到加速度约束方程；

同时得到作用在两体系统上的约束反力；

根据加速度约束方程和两体动力学方程联立，得到系统的动力学方程。

3.根据权利要求1所述的方法，其特征在于，所述S9包括：

在加速度约束方程中加入修正量，用绳长的变化量和绳长的导数进行反馈对约束方程进行修正，得到修正后的动力学方程。

4.根据权利要求1所述的方法，其特征在于，

所述气囊体坐标系定义为以气囊质心为原点，x轴在囊体纵对称面内指向前进方向为正，y轴在囊体纵对称面内竖直向上为正，z轴定义符合右手定则；

所述吊舱体坐标系定义为以吊舱质心为原点，x轴在吊舱纵对称面内指向前进方向为正，y轴在吊舱纵对称面内竖直向上为正，z轴定义符合右手定则；

发射系到气囊体系的坐标转换矩阵为：

发射系到吊舱体系的坐标转换矩阵：

式中，θ₁,ψ₁,γ₁分别为气囊的俯仰角，偏航角，滚转角；θ₂,ψ₂,γ₂分别为吊舱的俯仰角，偏航角，滚转角。

5.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-4中任一项所述的方法。

6.一种计算设备，包括处理器，其特征在于，所述处理器执行程序时实现如权利要求1-4中任一项所述的方法。