CN113119076B - 一种三自由度并联机构的姿态闭环反馈控制方法和系统 - Google Patents

Abstract

本发明公开了一种三自由度并联机构的姿态闭环反馈控制方法和系统，属于三自由度并联机构控制领域。包括：S1.当检测到动平台实际姿态角与目标姿态角偏差超过预设阈值时，基于姿态角偏差计算动平台xyz各轴的当前转矩，根据动平台xyz各轴的当前转矩解算出各个电动推杆的出力；S2.将解算出的各个电动推杆的出力施加到对应电动推杆；S3.重复步骤S1‑S2，直至动平台实际姿态角与目标姿态角偏差未超过设定阈值。本发明通过动平台绕静平台定坐标系三个轴的转矩通过解耦矩阵解算到三个电动推杆的出力，避免了三个电动推杆的出力F₁、F₂、F₃的耦合对绕下平台定坐标系转动的三个角度的影响，从而实现高精度稳定控制。

Description

一种三自由度并联机构的姿态闭环反馈控制方法和系统

技术领域

本发明属于三自由度并联机构控制领域，更具体地，涉及一种三自由度并联机构的姿态闭环反馈控制方法和系统。

背景技术

舰船三自由度并联机构早期出现主要由于战争的需要，至今已应用于卫星通信、舰载武器、海上舰载机以及直升机降落、海上科学考察以及资源勘探和海上人员输送及救援等领域。稳定平台功能实现一般为以下过程，首先采集舰船的位姿信号，通过预测算法根据前一段时间的位姿信号对下一阶段的舰船位姿进行预测，然后实时预测的位姿信号进行舰船运动补偿，达到隔离舰船的摇荡运动的效果，为工作对象提供一个相对惯性系稳定的工作环境。目前应用较为广泛的海洋探索领域的舰船稳定设备包括舰船辅助降落、舰船载武器稳定系统以及海上人员输送稳定平台等。

三自由度并联机构运动系统控制方法的优劣，将会对三自由度运动系统的特性产生很大影响，关系到能否充分发挥和挖掘其性能潜力，因此三自由度运动系统控制策略的研究是一项重要工作。基于并联机构的三自由度运动系统在各自由度的特性存在很大差异，而且各自由度之间存在很强的耦合作用。它的这些特性主要是由并联机构的结构复杂性和动力学的高度非线性引起的。目前，基于运动学反解的姿态闭环和位置闭环的控制算法对于高精度的并联机构运动控制领域响应不够快，控制参数难以调节，很难满足要求。

发明内容

针对现有技术的缺陷和改进需求，本发明提供了一种三自由度并联机构的姿态闭环反馈控制方法和系统，其目的在于以动平台(上平台)姿态作为反馈，三个电推杆作为驱动力的高精度闭环控制算法，力学解耦的方法消除各轴姿态角之间的耦合，具有工作稳定，响应过程快，稳态误差小的优势，满足高精度控制领域的技术需求。

为实现上述目的，按照本发明的第一方面，提供了一种三自由度并联机构的姿态闭环反馈控制方法，所述三自由度并联机构的动平台和静平台之间通过三个电动推杆连接，用于实现横滚角、俯仰角和偏航角三个姿态运动，该方法包括以下步骤：

S1.当检测到动平台实际姿态角与目标姿态角偏差超过预设阈值时，基于姿态角偏差计算动平台xyz各轴的当前转矩，根据动平台xyz各轴的当前转矩解算出各个电动推杆的出力；

S2.将解算出的各个电动推杆的出力施加到对应电动推杆；

S3.重复步骤S1-S2，直至动平台实际姿态角与目标姿态角偏差未超过设定阈值。

优选地，所述根据动平台xyz各轴的当前转矩解算出各个电动推杆的出力，包括以下步骤：

(1)根据动平台动坐标系下转换到静平台定坐标系下后动平台各电动推杆连接点坐标、静平台定坐标系下各电动推杆的推力单位力向量坐标、动平台中心点与静平台中心点的距离，确定第一电动推杆三轴方向的等效力臂k₁、k₄、k₇、第二电动推杆三轴方向的等效力臂k₂、k₅、k₈、第二电动推杆三轴方向的等效力臂k₃、k₆、k₉；

(2)通过动平台xyz各轴的当前转矩和上述等效力臂，解算出各个电动推杆的出力。

优选地，步骤(1)中，各等效力臂计算公式如下：

其中，(x_i，y_i，z_i)表示静平台定坐标系下第i个电动推杆的推力单位力向量坐标，(x_ii，y_ii，z_ii)表示动平台动坐标系下转换到静平台定坐标系下后动平台第i个电动推杆连接点坐标，H表示动平台中心点与静平台中心点的距离，i＝1，2，3。

优选地，所述动平台动坐标系下转换到静平台定坐标系下后动平台各电动推杆连接点坐标，通过以下方式获取：

(1)根据动平台实际三轴方向姿态角(θ_x，θ_y，θ_z)，计算旋转矩阵R；

(2)根据旋转矩阵，计算动平台动坐标系上的各个点A′，B′，C′转换到静平台定坐标系的各个点A″，B″，C″的坐标；

(x₁₁，y₁₁，z₁₁)＝R*A′+O′O

(x₂₂，y₂₂，z₂₂)＝R*B′+O′O

(x₃₃，y₃₃，z₃₃)＝R*C′+O′O

其中，O′O表动平台动坐标系原点坐标在静平台定坐标系中的坐标。

优选地，所述各个电动推杆的推力的单位力向量在静平台定坐标系下的坐标通过以下方式获取：

(x₁，y₁，z₁)＝A″-A

(x₂，y₂，z₂)＝B″-B

(x₃，y₃，z₃)＝C″-C

其中，A，B，C表示静平台定坐标系下下平台各电动推杆的连接点坐标。

优选地，解算出的三个电推杆的出力F₁、F₂、F₃的解耦关系式如下：

其中，T_x，T_y，T_z分别表示动平台xyz各轴的当前转矩。

为实现上述目的，按照本发明的第二方面，提供了一种三自由度并联机构的姿态闭环反馈控制系统，包括：计算机可读存储介质和处理器；

所述计算机可读存储介质用于存储可执行指令；

所述处理器用于读取所述计算机可读存储介质中存储的可执行指令，执行如第一方面所述的三自由度并联机构的姿态闭环反馈控制方法。

总体而言，通过本发明所构思的以上技术方案，能够取得以下有益效果：

本发明提出一种三自由度并联机构的姿态闭环反馈控制方法，通过上平台绕下平台定坐标系三个轴的转矩通过解耦矩阵解算到三个电动推杆的出力，从而实现将多输入、多输出系统转换为单输入单输出系统，即每个轴转矩T分别对对应轴的转角有影响，避免了三个电动推杆的出力F₁、F₂、F₃的耦合对绕下平台定坐标系转动的三个角度的影响，进而实现上平台绕三个轴的转动角度与设定的三个轴的角度误差保持在±1°以内。由于电动推杆电流环带宽较高，所以姿态稳定系统的响应速度提高。通过调节闭环控制系统中的PID控制器参数进而消除稳态误差，保证系统稳定性，最终提高稳定平台的姿态稳定性能。

附图说明

图1是本发明控制对象机械系统的结构示意图；

图2是本发明高精度闭环控制方法图；

图3是图1中的整体结构简单示意图；

图4是图1中的上平台球铰坐标示意图；

图5是图1中的下平台球铰坐标示意图；

图6是动坐标系向下平台定坐标系转换示意图；

图7是图1中的三个电动推杆出力作用上平台正视示意图；

图8是图1中的三个电动推杆出力作用上平台俯视示意图；

图9是三自由度并联平台下平台摇摆角度-时间关系图；

图10是三自由度并联平台上平台姿态角-时间关系图；

在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：1-上平台，2-第一电动推杆，3-第二电动推杆，4-下平台，5-中间支撑杆，6-第三电动推杆。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明提出一种三自由度并联机构，其包括：动平台、静平台(实施例中取上平台、下平台，下文均用上/下平台表示)、第一电动推杆、第二电动推杆、第三电动推杆、中间支撑杆、第一球铰、第二球铰、第三球铰、第四球铰、第五球铰、第六球铰、第七球铰及下平台。

所述第一电推杆上端和第一球铰通过螺栓连接，所述第一电推杆下端和第四球铰通过螺栓连接。

所述第二电推杆的上端和第二球铰通过螺栓连接，所述第二球铰下端和第五球铰通过螺栓连接。

所述第三电推杆的上端和第三球铰通过螺栓连接，所述第三电推杆下端和第六球铰通过螺栓连接。

中间支撑杆上端和第七球铰通过螺栓连接，中间支撑杆和下平台通过螺栓固定连接。

第一球铰和上平台第一球形槽通过压盖及螺钉连接，第二球铰和上平台第二球形槽通过压盖及螺钉连接，第三球铰和上平台第三球形槽通过压盖及螺钉连接，第四球铰和下平台第一球形槽通过压盖及螺钉连接，第五球铰和下平台第二球形槽通过压盖及螺钉连接，第六球铰和下平台第三球形槽通过压盖及螺钉连接，第七球铰和上平台第四球形槽通过压盖及螺钉连接。

本发明通过三个电动推杆呈正三角形分布支撑及固定杆中心支撑方式排布，能够实现稳定平台的横滚向、俯仰向以及偏航向的运动。

如图2所示，本发明提供了一种高精度闭环反馈控制方法，所述高精度闭环反馈控制方法，通过将角位移传感器测量得到的上平台的三个轴的转动角度R′_x、R′_y、R′_z和目标设定的R_x、R_y、R_z作差，得到e_x、e_y、e_z。通过PID算法得出分别绕三个轴转动的转矩T_x、T_y、T_z。通过解耦矩阵解算出三个电动推杆的应当给出的推力F₁、F₂、F₃，将得到的F₁施加到第一根电动推杆，的F₂施加到第二根电动推杆，将得到的F₃是施加到第三根电动推杆，那么也将得到即三根电动推杆的出力F₁、F₂、F₃作用到上平台，使得上平台绕下平台定坐标系三个坐标轴的转动角度R′_x、R′_y、R′_z发生变化。然后角位移传感器继续测量得到上平台绕三个轴转动的角度R′_x、R′_y、R′_z，然后重复上述过程，直至上平台的测量值和设定值一致。

图3是图1的整体结构简单示意图，设定上平台的半径为r，下平台的半径为R，整个平台的高度为H。则其核心部分的解耦算法，即T_x、T_y、T_z解算出F₁、F₂、F₃过程如下：

如图4所示，为上平台的三个球铰对应的三个点，设定上平台的三个球铰对应的点分别为A′、B′、C′，每两点之间的连线互成60°，那么在上平台对应的动坐标系x′y′z′-o′中的A′、B′、C′坐标分别为：

如图5所示，为下平台的三个球铰对应的三个点，设定下平台的三个球铰对应的点分别为A、B、C，每两点之间的连线互成60°，那么在下平台对应的定坐标系xyz-o中的A、B、C坐标分别为：

如图6所示，为上平台动坐标系向下平台动坐标系旋转变换的角度关系，即上平台动坐标系x′y′z′-o′绕下平台定坐标系xyz-o的x轴旋转α₁，上平台动坐标系x′y′z′-o′绕下平台定坐标系xyz-o的y轴旋转β₁，上平台动坐标系x′y′z′-o′绕下平台定坐标系xyz-o的z轴旋转γ₁，所以对应的旋转矩阵R为：

其中：

θ_x-上平台绕与下平台定坐标系x轴的相对姿态角；

θ_x-上平台绕与下平台定坐标系y轴的相对姿态角；

θ_x-上平台绕与下平台定坐标系z轴的相对姿态角；

根据动坐标系坐标转换到定坐标系坐标的关系式：P″＝R*P′+o′o；

其中：

P″-点在定坐标系中的坐标；

P′-点在动坐标系中的坐标；

R-动坐标系绕定坐标系旋转的旋转矩阵；

o′o-动坐标系原点坐标在定坐标系中的坐标，为(0，0，H)。

所以，根据上平台和下平台的相对姿态可以得出，上平台动坐标系上的各个点转换到下平台定坐标系的各个点的坐标为：

进而可以计算出，各个电动推杆的推力的单位力向量

分别为：

如图7所示，各个电动推杆的推力F₁、F₂、F₃分别为

将F₁、F₂、F₃分别沿定坐标系三个轴方向分解，进而可以得出：

如图8所示，三根电动推杆推力F₁、F₂、F₃分别作用到上平台的三个点A′、B′、C′，进一步的，上平台绕三个轴转动的力矩T_x、T_y、T_z与三个电推杆推力F₁、F₂、F₃的关系如下：

根据关系式(4)-(8)可以得出：

进一步合并化简可以得到：

不妨令：

那么，通过(9)和(10)可以得到：

所以，T_x、T_y、T_z解算到三个电推杆的出力F₁、F₂、F₃的解耦关系式为：

上述即完成了从上平台绕下平台定坐标系三个轴的转矩T_x、T_y、T_z解算到三个电动推杆的出力F₁、F₂、F₃解耦部分的计算；

进一步的，如图2所示，通过安装在上平台的角位移传感器测量出上平台实际的姿态角度R′_x、R′_y、R′_z与设定的上平台各个姿态角R_x、R_y、R_z做差值，得到误差e_x、e_y、e_z。经过PID控制器(比例、积分、微分控制器)后成为上平台绕下平台定坐标系转动的转矩T_x、T_y、T_z，然后通过上平台的角位移传感器测量出的绕下平台定坐标系三个轴的姿态角R′_x、R′_y、R′_z以及下平台的角位移传感器测量出来的下平台的摇摆的角度R″_x、R″_y、R″_z，可以测出上平台相对于下平台的相对姿态角θ_x、θ_y、θ_z，按照上述公式完成从上平台绕下平台定坐标系三个轴的转矩T_x、T_y、T_z解算到三个电动推杆的出力F₁、F₂、F₃解耦部分的计算，将三个电动推杆的出力F₁、F₂、F₃分别给到对应的电推杆，进而，电推杆的出力也就作用到上平台，从而完成了整个高精度闭环反馈控制。

所述PID经典控制算法，是通过将角位移传感器测量得到的上平台的三个轴的转动角度R′_x、R′_y、R′_z和目标设定的R_x、R_y、R_z作差，得到e_x、e_y、e_z，然后分别进行比例(P，Proportion)、积分(I，Integral)与微分(D，Differential)计算，将以上三者分别乘上比例系数、积分系数和微分系数后相加，其计算结果作为控制量对被控对象进行控制。该算法优点是结构简单、参数易调、适应性强。

最后，通过频域开环伯德图调节R_x姿态角对应的PID参数K_px、K_ix、K_dx，使得其幅值裕度在6dB左右，相位裕度在30°～45°左右，可以达到R_x姿态角稳定控制的效果；通过频域开环伯德图调节R_y姿态角对应的PID参数K_py、K_iy、K_dy，使得其幅值裕度在6dB左右，相位裕度在30°～45°左右，可以达到R_y姿态角稳定控制的效果；通过频域开环伯德图调节R_z姿态角对应的PID参数K_pz、K_iz、K_dz，使得其幅值裕度在6dB左右，相位裕度在30°～45°左右，可以达到R_z姿态角稳定控制的效果。

如图9所示，通过matlab/simmechanics仿真，给定上平台的角位移传感器的测量周期是0.001s，下平台的角位移传感器的测量周期是0.001s；同时给定下平台R″_x、R″_y、R″_z摇摆角度均为±10°，摇摆周期为0.3s；如图10所示，设定上平台的姿态角R_x、R_y、R_z均为0°，仿真结果得到的R_x、R_y、R_z姿态误差均在0.05°左右，达到了高精度闭环反馈控制的效果。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种三自由度并联机构的姿态闭环反馈控制方法，其特征在于，所述三自由度并联机构的动平台和静平台之间通过三个电动推杆连接，用于实现横滚角、俯仰角和偏航角三个姿态运动，该方法包括以下步骤：

S1.当检测到动平台实际姿态角与目标姿态角偏差超过预设阈值时，基于姿态角偏差计算动平台xyz各轴的当前转矩，根据动平台xyz各轴的当前转矩解算出各个电动推杆的出力；

S2.将解算出的各个电动推杆的出力施加到对应电动推杆；

S3.重复步骤S1-S2，直至动平台实际姿态角与目标姿态角偏差未超过设定阈值；

所述根据动平台xyz各轴的当前转矩解算出各个电动推杆的出力，包括以下步骤：

（1）根据动平台动坐标系下转换到静平台定坐标系下后动平台各电动推杆连接点坐标、静平台定坐标系下各电动推杆的推力单位力向量坐标、动平台中心点与静平台中心点的距离，确定第一电动推杆三轴方向的等效力臂

、第二电动推杆三轴方向的等效力臂

、第三电动推杆三轴方向的等效力臂

；

（2）通过动平台xyz各轴的当前转矩和上述等效力臂，解算出各个电动推杆的出力。

2.如权利要求1所述的方法，其特征在于，步骤（1）中，各等效力臂计算公式如下：

其中，

表示静平台定坐标系下第

个电动推杆的推力单位力向量坐标，

表示动平台动坐标系下转换到静平台定坐标系下后动平台第

个电动推杆连接点坐标，

表示动平台中心点与静平台中心点的距离，

。

3.如权利要求1或2所述的方法，其特征在于，所述动平台动坐标系下转换到静平台定坐标系下后动平台各电动推杆连接点坐标，通过以下方式获取：

（1）根据动平台实际三轴方向姿态角

，计算旋转矩阵

；

（2）根据旋转矩阵，计算动平台动坐标系上的上平台各个点

转换到静平台定坐标系的各个点

的坐标；

其中，

表动平台动坐标系原点坐标在静平台定坐标系中的坐标，

表示动平台动坐标系下转换到静平台定坐标系下后动平台第

个电动推杆连接点坐标，

。

4.如权利要求1或2所述的方法，其特征在于，所述各个电动推杆的推力的单位力向量在静平台定坐标系下的坐标通过以下方式获取：

其中，

表示静平台定坐标系下下平台各电动推杆的连接点坐标，

表示上平台各个点

转换到静平台定坐标系的坐标，

表示静平台定坐标系下第

个电动推杆的推力单位力向量坐标，

。

5.如权利要求1所述的方法，其特征在于，解算出的三个电推杆的出力

的解耦关系式如下：

其中，

分别表示动平台xyz各轴的当前转矩。

6.一种三自由度并联机构的姿态闭环反馈控制系统，其特征在于，包括：计算机可读存储介质和处理器；

所述计算机可读存储介质用于存储可执行指令；

所述处理器用于读取所述计算机可读存储介质中存储的可执行指令，执行如权利要求1至5任一项所述的三自由度并联机构的姿态闭环反馈控制方法。

Images