CN107656530A - 变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统 - Google Patents
变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统 Download PDFInfo
- Publication number
- CN107656530A CN107656530A CN201610594459.8A CN201610594459A CN107656530A CN 107656530 A CN107656530 A CN 107656530A CN 201610594459 A CN201610594459 A CN 201610594459A CN 107656530 A CN107656530 A CN 107656530A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mover
- mtd
- centerdot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 19
- 230000033001 locomotion Effects 0.000 claims abstract description 18
- 230000005484 gravity Effects 0.000 claims description 24
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 11
- 239000013598 vector Substances 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 10
- 230000009466 transformation Effects 0.000 claims description 10
- 238000013016 damping Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 230000002238 attenuated effect Effects 0.000 claims description 2
- 230000002153 concerted effect Effects 0.000 claims description 2
- 230000008859 change Effects 0.000 abstract description 4
- 238000006073 displacement reaction Methods 0.000 description 4
- 238000004088 simulation Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请涉及“变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统”。本发明的实施例涉及一种变参数开架式海洋水下机器人的路径跟踪控制，所述的变参数开架式海洋水下机器人具有4个自由度，并携带一个机械臂，由于所携带机械臂的姿态变化，导致开架式海洋机器人的参数是变化的，本发明通过重心公式实时计算出重心的位置，并在重心处建立变参数开架式海洋机器人的重心坐标，在此基础上建立变参数开架式海洋机器人的动力学模型，并采用加速度补偿控制方法进行变参数开架式海洋水下机器人的轨迹跟踪控制，实现变参数开架式海洋水下机器人的高精度运动控制。本发明的实施例提供的动力学建模以及运动控制对变参数开架式海洋水下机器人具有普遍实用性。
Description
技术领域
本发明涉及的是一种变参数开架式海洋水下机器人动力学建模及轨迹跟踪控制方案，特别是涉及加速速度补偿的轨迹跟踪控制方案。
技术背景
探索和开发海洋具有着重要的战略意义，海洋水下机器人应运而生，在海洋领域发挥着重要的作用。随着海洋水下机器人的不断发展，其应用领域也不断扩展，在海洋世界研究与开发、探索未知领域、科学实践考察和水下工程实施等等方面都得到前所未有的突破。民用方面，海洋水下机器人主要被用来实现海洋环境数据监测和海底资源探索等方面。军事方面，海洋水下机器人作为无人武器系统的重要组成部分，能实现长距离水下空间自主周围环境勘测、目标识别跟踪、情报收集传输，极大程度拓宽了水面舰船或潜艇的协同作战空间，使得海洋水下机器人成为了未来水下战争中执行特殊作战任务的重要手段之一。
由于海洋水下环境复杂，同时存在多种不可控因素，因此海洋水下机器人的轨迹跟踪控制问题是本领域的一个重要技术问题。目前海洋水下机器人的轨迹跟踪控制主要集中在常规形态水下机器人(主要为AUV)，开架式海洋水下机器人的轨迹跟踪控制方面的研究较少，特别是变参数开架式海洋水下机器人的轨迹跟踪控制问题，目前尚未有相关技术。
所需要的是一种解决变参数开架式海洋水下机器人的轨迹跟踪控制问题的技术方案。
发明内容
根据本发明的实施例，提供了一种变参数开架式海洋水下机器人的轨迹跟踪控制方法，包含以下步骤：
步骤1：建立变参数开架式海洋水下机器人的大地坐标系、局部坐标系以及重心坐标系，并建立变参数开架式海洋水下机器人运动参数在三种坐标系下的变换关系；
步骤2：根据步骤1所建立的坐标系以及变参数开架式海洋水下机器人的结构特征，对变参数开架式海洋水下机器人进行受力分析；
步骤3：在步骤1及步骤2的基础上，采用加速度补偿来进行变参数开架式海洋水下机器人的轨迹跟踪控制。
根据本发明的实施例，提供了一种用于变参数开架式海洋水下机器人的轨迹跟踪控制的控制系统，包括：
负反馈回路，用于对测得的变参数开架式海洋水下机器人的实际轨迹与期望轨迹间的误差进行控制；以及
补偿回路：用于进行加速度动态补偿，以便对变参数开架式海洋水下机器人进行控制。
根据本发明的实施例，提供了一种用于变参数开架式海洋水下机器人的轨迹跟踪控制的装置，配置成执行根据本发明实施例的方法。
根据本发明的实施例，提供了一种控制系统，包括根据本发明实施例的用于变参数开架式海洋水下机器人的轨迹跟踪控制的装置。
本发明的实施例具有如下有益效果：
1.为变参数开架式海洋机器人的动力学模型建立提供了一种可行的方案，从而为变参数开架式海洋机器人的控制提供了必要基础；
2.在变参数开架式海洋机器人的轨迹跟踪控制过程中，引入了加速度补偿，提高了变参数开架式海洋机器人的响应速度，同时提高了变参数开架式海洋机器人的轨迹跟踪控制精度；
3.本发明的实施例所提供的变参数开架式海洋机器人轨迹跟踪控制方法，不仅适用与本发明中所涉及到的4自由度变参数开架式海洋机器人，本发明的实施例所提供方法同样适用于其它类型变参数开架式海洋机器人的轨迹跟踪控制。
附图说明
图1是根据本发明实施例的变参数开架式海洋机器人坐标系统图；
图2是根据本发明实施例的控制系统示意图；
图3是根据本发明实施例的变参数开架式海洋水下机器人的轨迹跟踪控制方法的框图；
图4是根据本发明实施例的变参数开架式海洋机器人x轴方向位移；
图5是根据本发明实施例的变参数开架式海洋机器人x轴方向速度；
图6是根据本发明实施例的变参数开架式海洋机器人y轴方向位移；
图7是根据本发明实施例的变参数开架式海洋机器人y轴方向速度；
图8是根据本发明实施例的变参数开架式海洋机器人z轴方向位移；
图9是根据本发明实施例的变参数开架式海洋机器人z轴方向速度。
具体实施方式
本发明内容一种变参数开架式海洋机器人轨迹跟踪控制方法的具体实施方式如下：
如图1所示，在运动平台1上建立局部坐标系H-XYZ来表示变参数开架式海洋水下机器人的参数以及使机械臂2的重心参数化，原点H位于海洋机器人上表面的重心处，X轴沿着海洋机器人前进的方向，Y轴垂直于X轴并指向机器人右侧，Z轴垂直XY平面指向机器人底板。在建立变参数开架式海洋水下机器人运动平台局部坐标系时，同时建立以大地为参考对象的大地坐标系，大地坐标系以E发射点为原点，Eξ轴、Eη轴在水平面内，Eζ轴沿铅垂方向竖直向下。为便于动力学分析，设机器人的重心为O，并在重心处建立重心坐标系O-xyz，x、y及z轴平行局部坐标系的X、Y及Z轴；
在海洋机器人局部坐标系H-XYZ下，机器人运动平台1的重心是固定的，质量为m1，重心位置G1＝[x1,y1,z1]T，机械臂2的质量为m2，重心位置在不同位姿下是不同的，需要通过三维实体软件进行实时计算，计算所得重心坐标设为G1＝[x1,y1,z1]T，从而可以得到在机器人局部坐标系下整体机器人的重心坐标G＝[xg,yg,zg]T，如公式所示。
其中：m为机器人总质量，大小为机器人运动平台的质量与机械臂质量之和。
计算出机器人整体重心位置后，可以确定重心坐标系，从而机器人在局部坐标系下的结构参数即可以通过线性变换到重心坐标系中，如公式所示：
其中：XO，YO及ZO为某结构参数在重心坐标系下的坐标，XH、YH及ZH为相应结构参数在局部坐标系下的坐标。
在建立坐标系的基础上，可以得出重心坐标系到大地坐标系的速度、角速度转换矩阵分别如公式(3)、(4)所示：
其中：θ、ψ分别为机器人的横摇角、纵倾角、转艏角。
设机器人在大地坐标系下的位置坐标可以表示为x、y、z，因此机器人在大地坐标系下的位置与姿态向量分别为λ1＝[x y z]T以及 设机器人在重心坐标系下的速度及角速度分别为χ1＝[u v w]T以及χ2＝[p q r]T，通过公式(3)及公式(4)可以得到机器人在大地坐标系下的速度及角速度矢量，如公式所示：
变参数开架式海洋水下机器人的4个自由度分别代表进退、潜浮、横移以及转艏等4个动作；设置机器人的最大航速为3节；一般情况下，各个自由度之间是单独运动，且水下运行速度缓慢，可以认为其一阶、二阶粘性类耦合水动力系数较小进行建模；因此，变参数开架式海洋水下机器人的动力学模型可以表示为：
其中：m为变参数开架式海洋水下机器人质量，及为附加质量，Xu、Yv、Zw及Nr为线性阻尼系数，Xuu、Yvv、Zwww以及Nrr为二次阻尼系数，Fx、Fy、Fz以及Tz为沿着x、y、z轴的驱动力合力以及绕z轴的力矩，W为机器人的重力，B为机器人浮力。
为便于进行控制分析，方程(6)可以写为矩阵形式：
其中：
N1＝Xuu+Xu|u|u|u|-(W-B)sinθ；
N4＝Nrr+Nr|r|r|r|；
F＝(Fx Fy Fz Tz)T；
公式(7)所示的变参数开架式海洋水下机器人的动力学方程是基于重心坐标系的，而在实际轨迹规划以及跟踪控制过程中都是在基于大地坐标系的，为了实现对水下机器人的精确有效地控制，考虑将大地坐标系中描述的轨迹、控制参数转换到重心坐标系中。
变参数海洋水下机器人只有4个自由度，所以θ与ψ均不考虑，因此在大地坐标系中的位置参数及姿态参数可以表示为：λ＝[λ1 ψ]T，从而根据公式(7)可以得到大地坐标系下的动力学模型，如公式所示：
其中：TE→O、ac为变换矩阵及变换向量。
变参数开架式海洋机器人在水中运动的过程中，考虑到初始化坐标存在误差以及海洋下扰动等因素，对比预期设定的海洋水下机器人的轨迹与实际运动轨迹，会有误差存在，因此，为了实现对机器人的有效轨迹跟踪，必须在控制系统中对可能存在的误差进行修正、补偿。
为解决上述问题，设计出如图2所示的控制系统，主要由补偿回路和负反馈回路构成。负反馈回路：对测得的海洋水下机器人的实际轨迹与期望轨迹间的误差进行控制；补偿回路：根据机器人动力学特性对模型进行动态补偿，将运动分离处理，使得海洋水下机器人更加容易控制。
在大地坐标系中，通过轨迹规划得到变参数开架式海洋水下机器人的理论姿态参数λd＝[xd yd zd ψd]T。因此，可以定义变参数开架式海洋水下机器人的位置误差为：e＝λ-λd，
控制系统中补偿回路根据机器人动力学特性对模型进行加速度动态补偿，补偿力可以表示为：
其中：τ为补偿力矢量。
负反馈控制回路采用比例微分控制，因此系统的动力学方程为：
经过计算，可以得到误差动力学方程为：
式中：Kd，Kp为正定矩阵，用来调节模型参数；
通过适当调节Kd，Kp的取值可以实现误差衰减，从任何初始条件出发，均能够得到从而实现全局稳定的轨迹跟踪。
通过方程(11)及方程(8)经过计算，可以得到完整表达式，
图3针对变参数开架式海洋水下机器人提出了一种动力学建模及轨迹跟踪控制方法，具体过程为：
步骤1(301)：建立变参数开架式海洋水下机器人的大地坐标系、局部坐标系以及重心坐标系，建立变参数开架式海洋水下机器人运动参数在三种坐标系下的变换关系；
所涉及到的变参数开架式海洋水下机器人的大地坐标系、局部坐标系以及重心坐标系，建立过程为：在运动平台1上建立局部坐标系H-XYZ来架构变参数开架式海洋水下机器人的参数以及使机械臂2的重心参数化，原点H位于海洋机器人上表面的重心处，X轴沿着海洋机器人前进的方向，Y轴垂直于X轴并指向机器人右侧，Z轴垂直XY平面指向机器人底板。在建立变参数开架式海洋水下机器人运动平台1局部坐标系时，同时建立以大地为参考对象的大地坐标系，大地坐标系以E发射点为原点，Eξ轴、Eη轴在水平面内，Eζ轴沿铅垂方向竖直向下。为便于动力学分析，设机器人的重心为O，并在重心处建立重心坐标系O-xyz，x、y及z轴平行局部坐标系的X、Y及Z轴；
在海洋机器人局部坐标系H-XYZ下，机器人运动平台1的重心是固定的，质量为m1，重心位置G1＝[x1,y1,z1]T，机械臂2的质量为m2，重心位置在不同位姿下是不同的，需要通过三维实体软件进行实时计算，计算所得重心坐标设为G1＝[x1,y1,z1]T，从而可以得到在机器人局部坐标系下整体机器人的重心坐标G＝[xg,yg,zg]T，如公式所示：
其中：m为机器人总质量，大小为机器人运动平台1的质量与机械臂2质量之和。
步骤2(302)：根据变参数开架式海洋水下机器人的结构特征，对变参数开架式海洋水下机器人进行受力分析，并建立变参数开架式海洋水下机器人的动力学模型；
所涉及到的变参数开架式海洋水下机器人的动力学模型，变参数开架式海洋水下机器人的4个自由度分别代表进退、潜浮、横移以及转艏等4个动作；设置机器人的最大航速为3节；一般情况下，各个自由度之间是单独运动，且水下运行速度缓慢，可以认为其一阶、二阶粘性类耦合水动力系数较小进行建模；因此，变参数开架式海洋水下机器人的动力学模型可以表示为：
其中：m为变参数开架式海洋水下机器人质量，及为附加质量，Xu、Yv、Zw及Nr为线性阻尼系数，Xu|u|、Yv|v|、Zw|w|w以及Nr|r|为二次阻尼系数，Fx、Fy、Fz以及Tz为沿着x、y、z轴的驱动力合力以及绕z轴的力矩，W为机器人的重力，B为机器人浮力。
为便于进行控制分析，方程(2)可以写为矩阵形式：
其中：
N1＝Xuu+Xu|u|u|u|-(W-B)sinθ；
N4＝Nrr+Nr|r|r|r|；
F＝(Fx Fy Fz Tz)T；
变参数海洋水下机器人在大地坐标系中的位置参数及姿态参数可以表示为：λ＝[λ1ψ]T，从而可以得到大地坐标系下的动力学模型，如公式所示：
其中：TE→O、ac为变换矩阵及变换向量。
步骤3(303)：在步骤1及步骤2的基础上，采用加速度补偿来进行变参数开架式海洋水下机器人的轨迹跟踪控制；
所述轨迹跟踪控制涉及变参数开架式海洋水下机器人的轨迹跟踪控制装置，在控制过程中引入加速度补偿，根据步骤2中所述的大地坐标系下的动力学模型，补偿力公式为：
其中：τ为补偿力矢量。
仿真验证
下面举例说明，验证根据本发明的实施例的技术方案的有效性；
利用SIMULINK仿真软件对上述建立的水下机器人动力学模型和控制系统的运动控制进行仿真分析。假设机器人到达指定地点(10,10,10)，并悬停在该地点，机器人的理论轨迹采用直达法进行规划。为了验证控制系统的性能，假设水下机器人实际出发位置为(1,1,1)，Kd，Kp为对角矩阵，对角元素分别为50，5，由于机器人在水下具有大惯性，因此仿真步长为1秒。
仿真参数设定：根据计算所设计的一种变参数开架式海洋水下机器人的参数为：m＝315kg，W＝3087N，B＝2916N， Xu＝183.2Ns/m，Xu|u|＝243.6Ns2/m，Yv＝201.9Ns/m，Yv|v|＝388.7Ns2/m，Zw＝260.8Ns/m，Zw|w|＝422.5Ns2/m，Nr＝167.3Nsm，Nr|r|＝66.2Ns2，Iz＝579.6Nms2。
图4，图6，图8表明，机器人在控制装置的作用下，在三个坐标轴方向都达到了预定地点，其中x轴方向到达预定地点时间较快，y轴、z轴方向到达预定地点的时间较慢。图5、图7及图9表明，x轴方向运动速度较快，y轴、z轴方向运动速度较慢，与位移曲线相对应，同时表明，机器人在到达指定地点后，在控制装置作用下保持了悬停。通过以上结果分析可知，机器人在所设计的控制装置作用，完成了预期任务，说明了动力学分析及控制装置设计的准确性。
以上所述仅为本发明的具体实施例，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (10)
1.一种变参数开架式海洋水下机器人的轨迹跟踪控制方法，包含以下步骤：
步骤1：建立变参数开架式海洋水下机器人的大地坐标系、局部坐标系以及重心坐标系，并建立变参数开架式海洋水下机器人运动参数在三种坐标系下的变换关系；
步骤2：根据步骤1所建立的坐标系以及变参数开架式海洋水下机器人的结构特征，对变参数开架式海洋水下机器人进行受力分析；
步骤3：在步骤1及步骤2的基础上，采用加速度补偿来进行变参数开架式海洋水下机器人的轨迹跟踪控制。
2.根据权利要求1所述的方法，其中，步骤1进一步包括：在变参数开架式海洋水下机器人的运动平台上建立局部坐标系H-XYZ来架构整机的参数以及使机械臂的重心参数化，原点H位于海洋机器人上表面的重心处，X轴沿着海洋机器人前进的方向，Y轴垂直于X轴并指向机器人右侧，Z轴垂直XY平面指向机器人底板；在建立海洋机器人运动平台局部坐标系时，同时建立以大地为参考对象的大地坐标系，大地坐标系以E发射点为原点，Eξ轴、Eη轴在水平面内，Eζ轴沿铅垂方向竖直向下；机器人的重心为表示为O，并在重心处建立重心坐标系O-xyz，x、y及z轴平行局部坐标系的X、Y及Z轴；机器人在大地坐标系下的位置坐标表示为x、y、z，由此机器人在大地坐标系下的位置与姿态向量分别为λ1＝[x y z]T以及机器人在重心坐标系下的速度及角速度分别为χ1＝[u v w]T以及χ2＝[p qr]T，通过变换矩阵Tv及Tω得到机器人的速度及角速度矢量在大地坐标系及重心坐标系下的变换关系，所述变换关系表示为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>T</mi>
<mi>V</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>&theta;</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&theta;</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>T</mi>
<mi>&omega;</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&chi;</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&chi;</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>.</mo>
</mrow>
3.根据权利要求1所述的方法，进一步包括：在受力分析的基础上，建立变参数开架式海洋水下机器人的动力学模型，所述动力学模型表示为：
其中：m为变参数开架式海洋水下机器人质量，及为附加质量，Xu、Yv、Zw及Nr为线性阻尼系数，Xu|u|、Yv|v|、Zw|w|w以及Nr|r|为二次阻尼系数，Fx、Fy、Fz以及Tz为沿着x、y、z轴的驱动力合力以及绕z轴的力矩，W为机器人的重力，B为机器人浮力。
4.根据要求3所述的参数开架式海洋水下机器人的动力学模型，其中：所述动力学模型进一步表示为如下形式：
<mrow>
<mi>M</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mover>
<mi>h</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mo>+</mo>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>,</mo>
<mover>
<mi>h</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>I</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mi>F</mi>
</mrow>
其中：
<mrow>
<mover>
<mi>h</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<msup>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mi>u</mi>
</mtd>
<mtd>
<mi>v</mi>
</mtd>
<mtd>
<mi>w</mi>
</mtd>
<mtd>
<mi>r</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>;</mo>
</mrow>
<mrow>
<mover>
<mi>h</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mo>=</mo>
<msup>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mover>
<mi>u</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
<mtd>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
<mtd>
<mover>
<mi>w</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
<mtd>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>;</mo>
</mrow>
<mrow>
<mi>M</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>d</mi>
<mi>i</mi>
<mi>a</mi>
<mi>g</mi>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>m</mi>
<mo>+</mo>
<msub>
<mi>X</mi>
<mover>
<mi>u</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>m</mi>
<mo>+</mo>
<msub>
<mi>Y</mi>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>m</mi>
<mo>+</mo>
<msub>
<mi>Z</mi>
<mover>
<mi>w</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>m</mi>
<mo>+</mo>
<msub>
<mi>N</mi>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
<mrow>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>,</mo>
<mover>
<mi>h</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msup>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<msub>
<mi>N</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>N</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>N</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>N</mi>
<mn>4</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>;</mo>
</mrow>
N1＝Xuu+Xu|u|u|u|-(W-B)sinθ；
N4＝Nrr+Nr|r|r|r|；
F＝(Fx Fy Fz Tz)T；
水下机器人在大地坐标系中的位置参数及姿态参数表示为：λ＝[λ1 ψ]T，从而得到大地坐标系下的动力学模型，如下：
<mrow>
<mi>I</mi>
<msup>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mo>{</mo>
<mi>M</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>E</mi>
<mo>&RightArrow;</mo>
<mi>O</mi>
</mrow>
</msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>c</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>,</mo>
<mover>
<mi>h</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>}</mo>
<mo>=</mo>
<mi>F</mi>
</mrow>
其中：TE→O、ac为变换矩阵及变换向量。
5.根据权利要求1-4中任一项所述的方法，其中采用加速度补偿来进行变参数开架式海洋水下机器人的轨迹跟踪控制进一步包括：在跟踪控制过程中引入加速度补偿，补偿力公式为：
<mrow>
<mi>I</mi>
<msup>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mo>{</mo>
<mi>M</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>E</mi>
<mo>&RightArrow;</mo>
<mi>O</mi>
</mrow>
</msub>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mi>d</mi>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>c</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>,</mo>
<mover>
<mi>h</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>}</mo>
<mo>=</mo>
<mi>&tau;</mi>
</mrow>
其中：τ为补偿力向量。
6.一种用于变参数开架式海洋水下机器人的轨迹跟踪控制的控制系统，包括：
负反馈回路，用于对测得的变参数开架式海洋水下机器人的实际轨迹与期望轨迹间的误差进行控制；以及
补偿回路：用于进行加速度动态补偿，以便对变参数开架式海洋水下机器人进行控制。
7.如权利要求6所述的控制系统，其中，所述补偿回路根据机器人动力学特性对模型进行加速度动态补偿，补偿力表示为：
<mrow>
<mi>I</mi>
<msup>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mo>{</mo>
<mi>M</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>E</mi>
<mo>&RightArrow;</mo>
<mi>O</mi>
</mrow>
</msub>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mi>d</mi>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>c</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>,</mo>
<mover>
<mi>h</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>}</mo>
<mo>=</mo>
<mi>&tau;</mi>
</mrow>
其中：τ为补偿力矢量；
其中，所述负反馈控制回路采用比例微分控制，由此系统的动力学方程为：
<mrow>
<mi>I</mi>
<msup>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mo>{</mo>
<mi>M</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>E</mi>
<mo>&RightArrow;</mo>
<mi>O</mi>
</mrow>
</msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>c</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>,</mo>
<mover>
<mi>h</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>}</mo>
<mo>=</mo>
<mi>&tau;</mi>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>d</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>d</mi>
</msub>
<mo>-</mo>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>p</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>&lambda;</mi>
<mi>d</mi>
</msub>
<mo>-</mo>
<mi>&lambda;</mi>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
由此得到误差动力学方程为：
<mrow>
<mover>
<mi>e</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>d</mi>
</msub>
<mover>
<mi>e</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>p</mi>
</msub>
<mi>e</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
式中：Kd，Kp为正定矩阵，用来调节模型参数。
8.如权利要求7所述的控制系统，其中，通过适当调节Kd，Kp的取值可以实现误差衰减，从任何初始条件出发，均能够得到以便进行全局稳定的轨迹跟踪，由此进一步得到用于进行控制的完整表达式：
<mrow>
<mi>I</mi>
<msup>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mo>{</mo>
<mi>M</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>E</mi>
<mo>&RightArrow;</mo>
<mi>O</mi>
</mrow>
</msub>
<mo>(</mo>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mi>d</mi>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>d</mi>
</msub>
<mo>(</mo>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>d</mi>
</msub>
<mo>-</mo>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>p</mi>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>&lambda;</mi>
<mi>d</mi>
</msub>
<mo>-</mo>
<mi>&lambda;</mi>
</mrow>
<mo>)</mo>
<mo>)</mo>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>c</mi>
</msub>
<mo>)</mo>
<mo>+</mo>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>h</mi>
<mo>,</mo>
<mover>
<mi>h</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>}</mo>
<mo>=</mo>
<mi>F</mi>
<mo>.</mo>
</mrow>
9.一种用于变参数开架式海洋水下机器人的轨迹跟踪控制的装置，配置成执行根据权利要求1-5中任一项所述的方法。
10.一种控制系统，包括权利要求9所述的用于变参数开架式海洋水下机器人的轨迹跟踪控制的装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610594459.8A CN107656530B (zh) | 2016-07-26 | 2016-07-26 | 变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610594459.8A CN107656530B (zh) | 2016-07-26 | 2016-07-26 | 变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107656530A true CN107656530A (zh) | 2018-02-02 |
| CN107656530B CN107656530B (zh) | 2022-04-19 |
Family
ID=61127418
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610594459.8A Active CN107656530B (zh) | 2016-07-26 | 2016-07-26 | 变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107656530B (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108674616A (zh) * | 2018-07-11 | 2018-10-19 | 哈尔滨工程大学 | 一种自主水下航行器的回收方法 |
| CN109048891A (zh) * | 2018-07-25 | 2018-12-21 | 西北工业大学 | 基于自触发模型预测控制的中性浮力机器人姿态与轨迹控制方法 |
| CN110032198A (zh) * | 2019-04-30 | 2019-07-19 | 苏州蓝智特种机器人有限公司 | 一种水下特种机器人艇-手多体动力学建模方法及装置 |
| CN110083941A (zh) * | 2019-04-28 | 2019-08-02 | 哈尔滨工程大学 | 一种五自由度rov非线性运动学建模方法 |
| CN110427043A (zh) * | 2019-09-04 | 2019-11-08 | 福州大学 | 基于作业飞行机器人重心偏移的位姿控制器设计方法 |
| CN113465822A (zh) * | 2021-07-02 | 2021-10-01 | 乐聚(深圳)机器人技术有限公司 | 基于多刚体机器人的质心位置计算方法、装置及介质 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070200525A1 (en) * | 2004-02-25 | 2007-08-30 | The Ritsumeikan Trust | Control system of floating mobile body |
| CN102519450A (zh) * | 2011-12-12 | 2012-06-27 | 东南大学 | 一种用于水下滑翔器的组合导航装置及方法 |
| CN104076688A (zh) * | 2014-07-17 | 2014-10-01 | 山东省科学院海洋仪器仪表研究所 | 一种自主式水下航行器主从式协同控制方法 |
| US20150175248A1 (en) * | 2002-07-08 | 2015-06-25 | Pelagic Pressure Systems | Dive Computer with Global Positioning System Receiver |
| CN105259902A (zh) * | 2015-09-06 | 2016-01-20 | 江苏科技大学 | 水下机器人惯性导航方法及系统 |
| CN105549611A (zh) * | 2014-10-30 | 2016-05-04 | 中国科学院沈阳自动化研究所 | 自主水下机器人的海洋环境自识别的航迹精确跟踪方法 |
-
2016
- 2016-07-26 CN CN201610594459.8A patent/CN107656530B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150175248A1 (en) * | 2002-07-08 | 2015-06-25 | Pelagic Pressure Systems | Dive Computer with Global Positioning System Receiver |
| US20070200525A1 (en) * | 2004-02-25 | 2007-08-30 | The Ritsumeikan Trust | Control system of floating mobile body |
| CN102519450A (zh) * | 2011-12-12 | 2012-06-27 | 东南大学 | 一种用于水下滑翔器的组合导航装置及方法 |
| CN104076688A (zh) * | 2014-07-17 | 2014-10-01 | 山东省科学院海洋仪器仪表研究所 | 一种自主式水下航行器主从式协同控制方法 |
| CN105549611A (zh) * | 2014-10-30 | 2016-05-04 | 中国科学院沈阳自动化研究所 | 自主水下机器人的海洋环境自识别的航迹精确跟踪方法 |
| CN105259902A (zh) * | 2015-09-06 | 2016-01-20 | 江苏科技大学 | 水下机器人惯性导航方法及系统 |
Non-Patent Citations (5)
| Title |
|---|
| 卢淑娟等: "基于嵌入式的水下航行器导航系统设计", 《航空计算技术》 * |
| 范士波等: "基于水动力试验的深海开架式潜水器三维轨迹跟踪控制平台设计(英文)", 《船舶力学》 * |
| 谢海斌等: "仿鱼鳍推进水下机器人流体动力测试系统", 《测试技术学报》 * |
| 边宇枢等: "6自由度水下机器人动力学分析与运动控制", 《机械工程学报》 * |
| 霍聪等: "潜伏式高速船型变航态自航数值模拟", 《船舶工程》 * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108674616A (zh) * | 2018-07-11 | 2018-10-19 | 哈尔滨工程大学 | 一种自主水下航行器的回收方法 |
| CN109048891A (zh) * | 2018-07-25 | 2018-12-21 | 西北工业大学 | 基于自触发模型预测控制的中性浮力机器人姿态与轨迹控制方法 |
| CN109048891B (zh) * | 2018-07-25 | 2021-12-07 | 西北工业大学 | 基于自触发模型预测控制的中性浮力机器人姿态与轨迹控制方法 |
| CN110083941A (zh) * | 2019-04-28 | 2019-08-02 | 哈尔滨工程大学 | 一种五自由度rov非线性运动学建模方法 |
| CN110032198A (zh) * | 2019-04-30 | 2019-07-19 | 苏州蓝智特种机器人有限公司 | 一种水下特种机器人艇-手多体动力学建模方法及装置 |
| CN110427043A (zh) * | 2019-09-04 | 2019-11-08 | 福州大学 | 基于作业飞行机器人重心偏移的位姿控制器设计方法 |
| CN113465822A (zh) * | 2021-07-02 | 2021-10-01 | 乐聚(深圳)机器人技术有限公司 | 基于多刚体机器人的质心位置计算方法、装置及介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107656530B (zh) | 2022-04-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107656530A (zh) | 变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统 | |
| Mohan et al. | Indirect adaptive control of an autonomous underwater vehicle-manipulator system for underwater manipulation tasks | |
| CN109634307A (zh) | 一种无人水下航行器复合航迹跟踪控制方法 | |
| CN112241125B (zh) | 一种基于微分平坦特性的无人机轨迹跟踪方法 | |
| Meng et al. | Space robots with flexible appendages: dynamic modeling, coupling measurement, and vibration suppression | |
| CN105807789B (zh) | 基于t-s模糊观测器补偿的uuv控制方法 | |
| CN106985139A (zh) | 基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法 | |
| CN105929842A (zh) | 一种基于动态速度调节的欠驱动uuv平面轨迹跟踪控制方法 | |
| CN107479566A (zh) | 基于三段式路径规划的挠性卫星姿态控制方法 | |
| CN106737659A (zh) | 一种水下无人航行器和机械手系统的手艇协调控制方法 | |
| CN108663939A (zh) | 考虑通讯丢包的uuv集群水平面路径跟踪控制方法 | |
| Dai et al. | Sliding Mode Impedance Control for contact intervention of an I-AUV: Simulation and experimental validation | |
| CN108663938A (zh) | 一种考虑通讯拓扑变换的uuv集群协调控制方法 | |
| CN109947126A (zh) | 四旋翼无人机的控制方法、装置、设备及可读介质 | |
| CN111857165B (zh) | 一种水下航行器的轨迹跟踪控制方法 | |
| Haugaløkken et al. | Experimental validation of end-effector stabilization for underwater vehicle-manipulator systems in subsea operations | |
| CN108829132A (zh) | 一种考虑大时滞的多uuv空间机动控制方法 | |
| CN108829126A (zh) | 一种考虑通讯延时的auv集群协调控制方法 | |
| CN107894775A (zh) | 一种欠驱动无人水下航行器轨迹生成与控制方法 | |
| Jin et al. | Back-stepping control design for an underwater robot with tilting thrusters | |
| Ghommam et al. | Distance‐Based Formation Control for Quadrotors with Collision Avoidance via Lyapunov Barrier Functions | |
| CN106570296A (zh) | 一种六自由度并联机构实时正解方法 | |
| Pang et al. | Multi-AUV formation reconfiguration obstacle avoidance algorithm based on affine transformation and improved artificial potential field under ocean currents disturbance | |
| Worrall et al. | Autonomous planetary rover control using inverse simulation | |
| Zhu et al. | Use of dynamic scaling for trajectory planning of floating pedestal and manipulator system in a microgravity environment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220520 Address after: 518000 1st floor, building a, No.8 Huanzhen Road, Digang community, Shajing street, Bao'an District, Shenzhen City, Guangdong Province Patentee after: SHENZHEN LIHE PRECISION EQUIPMENT TECHNOLOGY Co.,Ltd. Address before: 518000 1501, floor 15, block C, building 9, Baoneng Technology Park, Qinghu Industrial Park, Qingxiang Road, Longhua new area, Shenzhen, Guangdong Patentee before: SHENZHEN HUAQING PRECISION TECHNOLOGY Co.,Ltd. |
|
| TR01 | Transfer of patent right |