CN107145640A - 中性浮力实验中漂浮基座与机械臂的动态标度规划方法 - Google Patents

中性浮力实验中漂浮基座与机械臂的动态标度规划方法 Download PDF

Info

Publication number
CN107145640A
CN107145640A CN201710237568.9A CN201710237568A CN107145640A CN 107145640 A CN107145640 A CN 107145640A CN 201710237568 A CN201710237568 A CN 201710237568A CN 107145640 A CN107145640 A CN 107145640A
Authority
CN
China
Prior art keywords
mrow
msub
mover
centerdot
mtr
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
Application number
CN201710237568.9A
Other languages
English (en)
Other versions
CN107145640B (zh
Inventor
朱战霞
张光辉
宋江舟
袁建平
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201710237568.9A priority Critical patent/CN107145640B/zh
Publication of CN107145640A publication Critical patent/CN107145640A/zh
Application granted granted Critical
Publication of CN107145640B publication Critical patent/CN107145640B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design

Abstract

一种中性浮力实验中漂浮基座与机械臂的动态标度规划方法，包括以下步骤：第一步、在水池坐标系中设定机械臂末端执行器的轨迹，并在轨迹内插入一个路径点序列，进而将轨迹划分为n段执行；第二步、建立n个三阶多项式方程，将路径点计算成相应的关节变量q(t)；第三步、求解标定因子；第四步、对重新标定的机械臂轨迹进行求解，在每段关节空间轨迹中通过相同的规则分别求得轨迹动态标度，形成多个新的轨迹。本发明通过对运动轨迹的动态时间标定，形成新的机械臂关节角速度和角角速度，从而改变机械臂的惯性力和环境力，使得原先无法保持基座位姿稳定的规划方法实现基座位姿的稳定。

Description

中性浮力实验中漂浮基座与机械臂的动态标度规划方法
技术领域
本发明涉及空间微重力模拟领域，具体涉及一种中性浮力实验中漂浮基座与机械臂的动态标度规划方法，通过改变机械臂的惯性力和环境力，使基座位姿保持稳定。
背景技术
中性浮力实验是航天领域新兴起的一种研究方法。所谓中性浮力是指当物体处于液体中时，如果物体的密度与液体的密度相同，则该物体能够在液体中的任意一点悬浮。中性浮力模拟空间微重力效应的方法也称为液体浮力平衡重力方法，就是利用液体对物体的浮力抵消物体的重力，使物体处于悬浮状态。已经公开的技术有：Yuichiro Taira,Shinichi Sagara,Masahiro Oya.A robust controller with integral action forunderwater vehicle-manipulator systems including thruster dynamics[C].Kumamoto,Japan:International Conference on Advanced Mechatronic Systems,2014:415-420在已知UVMS和推力器的动力学参数，而对水下有界扰动不确定的前提下，设计一种鲁棒控制器，成功克服推进器的非线性对控制的不利影响，并且针对鲁棒控制器的缺点在控制器中增加了积分环节。Norimitsu Sakagami,Mizuho Shibata,SadaoKawamura.An attitude control system for underwater vehicle-manipulatorsystems[C].Alaska,USA:IEEE International Conference on Robotics andAutomation,2010:1761-1767.控制浮心和重心的相对位置来控制UVMS的姿态，改变浮心通过移动浮块来实现，文中设计的这套控制系统用于控制UVMS的俯仰角，俯仰角的变化范围从120°至150°，还能够在执行任务时保持整体系统的姿态。B.Siciliano,L.Sciavicco,L.Villani,G.Oriolo.Robotics:Modelling,Planning and Control[M].New York:Springer,2009.介绍了针对固定基座机械臂的动态标度方法，解决机械臂关节力矩不足以实现期望轨迹的问题。但是在自由漂浮系统下，上述研究方法均未考虑基座的推力及其推力矩不足的情况。根据任务的需要，机械臂的轨迹一般是需要保持在某一位姿上的。但由于机械臂运动时会对基座产生干扰，如果基座维持位姿所需的力和力矩大于螺旋桨推进器所能提供的最大推力和推力矩，那么基座在水池坐标系中的实际位置和姿态指向与期望位置和姿态指向之间将产生较大的误差，从而影响机械臂末端执行器在水池坐标系中的轨迹。
发明内容
本发明的目的在于针对上述现有技术中的问题，提供一种中性浮力实验中漂浮基座与机械臂的动态标度规划方法，对机械臂关节生成新的轨迹，调节机械臂关节角速度和角加速度，改变机械臂的惯性力和环境力，从而减小机械臂的运动对基座的动力学影响，使基座稳定。
为了实现上述目的，本发明采用的技术方案包括以下步骤：
第一步、在水池坐标系中设定机械臂末端执行器的轨迹，并在轨迹内插入一个路径点序列，进而将轨迹划分为n段执行；
第二步、在n段轨迹上建立n个三阶多项式方程，将每段轨迹通过逆运动学方程计算成相应的关节变量q(t)；
第三步、求解标定因子；
3.1)求解机械臂的期望轨迹，根据任务要求确定基座的期望位置、期望姿态以及期望运动状态；
3.2)将基座和机械臂的期望运动参数代入递归算法进行动力学逆解，确定轨迹；机械臂对基座产生的合力以及合力矩表示为τi(t)；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力及合力矩表示为τw(t)；
3.3)把τs(t)和τw(t)通过公式分配给各个推进剂，分别表示为τspk(t)和τwpk(t)，其中k＝1,2,.....,6，求解分配到推进器k的|τspk(t)|在时间ti-1和ti之间的最大值|τspkmax(tkm)|，τwpk(t)在此时刻的值为τwpk(tkm)；
3.4)已知每个理想推进器的最大推力均为τpmax，每个推进器的标度因子ci(k)为：
3.5)选择六个标度因子的最大值max(ci(k))作为机械臂期望轨迹的标度因子ci；
第四步、对重新标定的机械臂轨迹进行求解，在每段关节空间轨迹中通过相同的规则分别求得轨迹动态标度，形成多个新的轨迹，路径相同而时间规律不同的运动轨迹表达式如下：
第一步轨迹路径的参数表达式为：P＝f(s)；其中，s为路径长度，P为末端执行器在水池坐标系中的坐标；当t＝0时s＝0，当t＝tf时s＝sf。
第二步n个三阶多项式方程定义为Πi(t)，i＝1,2,…,n，通用约束条件为：
Πi(ti-1)＝qei-1；
Πi(ti)＝qei；
对应速度根据下列规则进行计算：
其中，给出时间区间[tk-1,tk]内分段的斜率；
选择三次多项式：q(t)＝a3t3+a2t2+a1t+a0
得到抛物线的速度曲线：
通过求解如下方程，得到关节变量q(t)：
步骤3.2)中机械臂对基座产生的合力包括惯性力、流体力、浮力和重力；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力包括线缆力、流体力、重力和浮力。
第四步对重新标定的机械臂轨迹进行求解包括以下步骤：
其中，r(t)是使用轨迹动态标度形成的时间规律函数，q(t)是机械臂规划求出的机械臂关节运动轨迹；对于定标函数，选择简单的线性函数形式：
R(t)＝ct；
其中c是一个大于1的常数，且
对公式进行两次求导，得到关系式：
机械臂动力学方程中由速度和加速度产生的项可以表示成：
将相关公式代入，能够得到函数r(t)重新标定过的机械臂动力学方程为：
把其中后两项线性化得到关系式：
公式变为：
机械臂动力学方程中与速度和加速度有关的项以c2的比例被减小或增大。
与现有技术相比，本发明具有如下的有益效果：某些任务要求下，机械臂的轨迹是建立在基座始终保持在某一位姿这一假设上的。基座的位姿稳定是依靠基座本身的静稳定性和螺旋桨推进器输出的推力来维持，而如果基座稳定所需的力和力矩大于螺旋桨推进器所能提供的最大推力和最大推力矩，那么基座就无法保持位姿的稳定。本发明通过对运动轨迹的动态时间标定，形成新的机械臂关节角速度和角加速度，从而改变机械臂的惯性力和环境力，使得原先无法保持基座位姿稳定的规划方法实现基座位姿的稳定。通过仿真实验证实，本发明能够使基座三个自由度的位置误差及姿态误差始终满足要求，实用性以及可实施性较强。
附图说明
图1本发明规划方法的整体流程图；
图2未进行动态标度的仿真实验结果数据统计图：
(a)基座位置数据图；(b)基座姿态数据图；
图3未进行动态标度的仿真实验中推进器推力数据图：
(a)1-3号推进器推力数据图；(b)4-6号推进器推力数据图；
图4使用动态标度的仿真实验结果数据统计图：
(a)Oxy平面轨迹示意图；(b)Oxz平面轨迹示意图；
图5使用动态标度的仿真实验结果数据统计图：
(a)关节角数据统计图；(b)关节角速度数据统计图；
图6使用动态标度的仿真实验结果数据统计图：
(a)基座位置数据图；(b)基座姿态数据图；
图7使用动态标度的仿真实验结果数据统计图：
(a)1-3号推进器推力数据图；(b)4-6号推进器推力数据图。
具体实施方式
下面结合附图对本发明做进一步的详细说明。
参见图1，本发明动态标度规划方法包括以下步骤：
第一步、在水池坐标系中设定机械臂末端执行器的轨迹，并在轨迹内插入一个路径点序列，进而将轨迹划分为n段执行；
路径的参数表达式为：P＝f(s)；其中，s为路径长度，P为末端执行器在水池坐标系中的坐标；当t＝0时s＝0，当t＝tf时s＝sf。
第二步、在n段轨迹上建立n个三阶多项式方程，将每段轨迹通过逆运动学方程计算成相应的关节变量q(t)；
n个三阶多项式方程定义为Πi(t)，i＝1,2,…,n，通用约束条件为：
Πi(ti-1)＝qei-1；
Πi(ti)＝qei；
对应速度根据下列规则进行计算：
其中，给出时间区间[tk-1,tk]内分段的斜率；
选择三次多项式：q(t)＝a3t3+a2t2+a1t+a0
得到抛物线的速度曲线：
通过求解如下方程，得到关节变量q(t)：
第三步、求解标定因子；
3.1)求解机械臂的期望轨迹，根据任务要求确定基座的期望位置、期望姿态以及期望运动状态；
3.2)将基座和机械臂的期望运动参数代入递归算法进行动力学逆解，确定轨迹；机械臂对基座产生的惯性力、流体力、浮力和重力的合力以及合力矩表示为τi(t)；基座在水下期望位置、期望姿态以及期望运动状态下受到的线缆力、流体力、重力和浮力的合力及合力矩表示为τw(t)；
3.3)把τs(t)和τw(t)通过公式分配给各个推进剂，分别表示为τspk(t)和τwpk(t)，其中k＝1,2,.....,6，求解分配到推进器k的|τspk(t)|在时间ti-1和ti之间的最大值|τspkmax(tkm)|，τwpk(t)在此时刻的值为τwpk(tkm)；
3.4)已知每个理想推进器的最大推力均为τpmax，每个推进器的标度因子ci(k)为：
3.5)选择六个标度因子的最大值max(ci(k))作为机械臂期望轨迹的标度因子ci；
第四步、对重新标定的机械臂轨迹进行求解，在每段关节空间轨迹中通过相同的规则分别求得轨迹动态标度，形成多个新的轨迹：
其中，r(t)是使用轨迹动态标度形成的时间规律函数，q(t)是机械臂规划求出的机械臂关节运动轨迹；对于定标函数，选择简单的线性函数形式：
R(t)＝ct；
其中c是一个大于1的常数，且
对公式进行两次求导，得到关系式：
机械臂动力学方程中由速度和加速度产生的项可以表示成：
将相关公式代入，能够得到函数r(t)重新标定过的机械臂动力学方程为：
把其中后两项线性化得到关系式：
公式变为：
上式给出了路径相同而时间规律不同的运动轨迹表达式。机械臂动力学方程中与速度和加速度有关的项以c2的比例被减小或增大。
实施例
在这组仿真实验中设置基座的初始位置坐标为[0 0 0]，基座的初始姿态为[0°0° 0°]，机械臂末端执行器的初始关节角为[-30° 30°]。任务目标是使机械臂末端执行器到达坐标[0.5 0.5 0.2]，同时基座位置保持在[0 0 0]。仿真中的位置统一用水池坐标系中的坐标表示，单位是米。姿态统一用欧拉角表示，单位是度。在仿真试验中基座使用基于趋近率的位置跟踪滑膜控制器，参数c设为diag(2.1,2.1,2.1,8,8,8)，参数b设为diag(5.8,5.8,5.8,20,20,20)。机械臂使用PID控制器给关节角输出控制信号，控制器增益系数Kp设为diag(300,200)，微分增益系数Kd设置为diag(70,50)，积分增益系数Ki设为diag(0.1,0.1)。
任务协同仿真过程可分为：
(1)基座从0秒开始运动，根据前面规划的运动轨迹在运动到期望姿态，轨迹的期望运动时间为10s。
(2)根据末端执行器和目标点在水池坐标系中的坐标得到机械臂末端执行器的操作空间轨迹。在操作空间轨迹中内插5个点，将其分为6段，机械臂将在关节空间中执行这六段轨迹，机械臂完整轨迹的期望运动时间为4s。
(3)使用动态标度方法对机械臂的轨迹进行重新定。标得到新的机械臂关节空间运动轨迹，完整轨迹的期望运动时间也随之变化，机械臂沿着新的轨迹进行运动。
(4)在机械臂末端执行器到达目标点后，基座和机械臂保持位置和姿态直到仿真实验结束，仿真将在第25秒结束。
首先对基座与机械臂进行任务协同仿真，不执行上述中的第(3)步，执行任务协同仿真的第(1)(2)(3)步，即不对机械臂进行重新标定，仿真结果如图2(a)、图2(b)、图3(a)以及图3(b)所示，从图中能够看出多个推进器的推力输出曲线都远远超出了极限推力10N。分析图中的数据，x向位置误差最大值有0.068m，y向位置误差最大值有0.120m，z向位置误差最大值有0.0027m，横滚角误差的最大值为3.08°，俯仰角θ误差的最大值为1.15°，航向角ψ误差的最大值为3.63°。从中还能够发现，位姿误差的最大值都出现在机械臂达到期望关节角时，这就说明在机械臂末端执行器本该到达目标点时，而末端执行器的位置误差处于较大值。在这种情况下，由于所需的推力超出了螺旋桨的极限推力，会使得基座的位姿无法保持稳定，导致任务失败。
为了完成目标，需要保持基座的位姿稳定。这里使用动态标度的方法来实现需要的轨迹规划。执行任务协同仿真的第(1)(2)(3)(4)步，仿真结果如图4(a)，图4(b)，图5(a)，图5(b)，图6(a)，图6(b)，图7(a)，图7(b)所示。
使用动态标度后，从图中能够看出基座三个自由度的位置误差始终小于3×10-3m，且基座三个转动自由度的姿态误差始终小于0.38°，而基座的期望姿态角为[15.79° 0° -45°]。从图中能够看出推力输出量最大的3号螺旋桨推进器的推进曲线比较好的限制在了10N内，使得螺旋桨能够对基座位姿的稳定进行有效的控制。

Claims (6)

1.一种中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于：
第一步、在水池坐标系中设定机械臂末端执行器的轨迹，并在轨迹内插入一个路径点序列，进而将轨迹划分为n段执行；
第二步、在n段轨迹上建立n个三阶多项式方程，将每段轨迹通过逆运动学方程计算成相应的关节变量q(t)；
第三步、求解标定因子；
3.1)求解机械臂的期望轨迹，根据任务要求确定基座的期望位置、期望姿态以及期望运动状态；
3.2)将基座和机械臂的期望运动参数代入递归算法进行动力学逆解，确定轨迹；机械臂对基座产生的合力以及合力矩表示为τi(t)；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力及合力矩表示为τw(t)；
3.3)把τs(t)和τw(t)通过公式分配给各个推进剂，分别表示为τspk(t)和τwpk(t)，其中k＝1,2,.....,6，求解分配到推进器k的|τspk(t)|在时间ti-1和ti之间的最大值|τspkmax(tkm)|，τwpk(t)在此时刻的值为τwpk(tkm)；
3.4)已知每个理想推进器的最大推力均为τpmax，每个推进器的标度因子ci(k)为：
<mrow> <msub> <mi>c</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>{</mo> <mrow> <mtable> <mtr> <mtd> <mrow> <mo>=</mo> <msqrt> <mfrac> <mrow> <mo>|</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>s</mi> <mi>p</mi> <mi>k</mi> <mi>max</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mi>m</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>|</mo> </mrow> <mrow> <msub> <mi>&amp;tau;</mi> <mrow> <mi>p</mi> <mi>max</mi> </mrow> </msub> <mo>-</mo> <mo>|</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mi>m</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>|</mo> </mrow> </mfrac> </msqrt> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msqrt> <mfrac> <mrow> <mo>|</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>s</mi> <mi>p</mi> <mi>k</mi> <mi>max</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mi>m</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>|</mo> </mrow> <mrow> <msub> <mi>&amp;tau;</mi> <mrow> <mi>p</mi> <mi>max</mi> </mrow> </msub> <mo>+</mo> <mo>|</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mi>m</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>|</mo> </mrow> </mfrac> </msqrt> </mrow> </mtd> </mtr> </mtable> <mo>,</mo> <mtable> <mtr> <mtd> <mrow> <mi>sgn</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>s</mi> <mi>p</mi> <mi>k</mi> <mi>max</mi> </mrow> </msub> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mi>m</mi> </mrow> </msub> <mo>)</mo> <mo>)</mo> </mrow> <mo>=</mo> <mi>sgn</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>w</mi> <mi>p</mi> <mi>k</mi> </mrow> </msub> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mi>m</mi> </mrow> </msub> <mo>)</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sgn</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>s</mi> <mi>p</mi> <mi>k</mi> <mi>max</mi> </mrow> </msub> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mi>m</mi> </mrow> </msub> <mo>)</mo> <mo>)</mo> </mrow> <mo>&amp;NotEqual;</mo> <mi>sgn</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>w</mi> <mi>p</mi> <mi>k</mi> </mrow> </msub> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mi>m</mi> </mrow> </msub> <mo>)</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mrow> <mo>;</mo> </mrow>
3.5)选择六个标度因子的最大值max(ci(k))作为机械臂期望轨迹的标度因子ci；
第四步、对重新标定的机械臂轨迹进行求解，在每段关节空间轨迹中通过相同的规则分别求得轨迹动态标度，形成多个新的轨迹，路径相同而时间规律不同的运动轨迹表达式如下：
<mrow> <msub> <mi>&amp;tau;</mi> <mi>s</mi> </msub> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>c</mi> <mn>2</mn> </msup> <mover> <msub> <mi>&amp;tau;</mi> <mi>s</mi> </msub> <mo>&amp;OverBar;</mo> </mover> <mrow> <mo>(</mo> <mi>c</mi> <mi>t</mi> <mo>)</mo> </mrow> <mo>.</mo> </mrow>
2.根据权利要求1所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，第一步轨迹路径的参数表达式为：P＝f(s)；其中，s为路径长度，P为末端执行器在水池坐标系中的坐标；当t＝0时s＝0，当t＝tf时s＝sf。
3.根据权利要求1所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，第二步n个三阶多项式方程定义为Πi(t)，i＝1,2,…,n，通用约束条件为：
Πi(ti-1)＝qei-1；
Πi(ti)＝qei；
<mrow> <msub> <mover> <mo>&amp;Pi;</mo> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>;</mo> </mrow>
<mrow> <msub> <mover> <mo>&amp;Pi;</mo> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mo>;</mo> </mrow>
对应速度根据下列规则进行计算：
<mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>o</mi> </mrow> </msub> <mo>=</mo> <mn>0</mn> <mo>;</mo> </mrow> 1
<mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mrow> <mo>{</mo> <mrow> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>k</mi> <mi>b</mi> </msubsup> <mo>+</mo> <msubsup> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>b</mi> </msubsup> <mo>)</mo> </mrow> <mi>sgn</mi> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> <mtable> <mtr> <mtd> <mrow> <mi>sgn</mi> <mrow> <mo>(</mo> <msub> <mi>v</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>sgn</mi> <mrow> <mo>(</mo> <msub> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sgn</mi> <mrow> <mo>(</mo> <msub> <mi>v</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>&amp;NotEqual;</mo> <mi>sgn</mi> <mrow> <mo>(</mo> <msub> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mrow> </mrow> <mo>;</mo> </mrow>
<mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>n</mi> </mrow> </msub> <mo>=</mo> <mn>0</mn> <mo>;</mo> </mrow>
其中，给出时间区间[tk-1,tk]内分段的斜率；
选择三次多项式：q(t)＝a3t3+a2t2+a1t+a0
得到抛物线的速度曲线：
通过求解如下方程，得到关节变量q(t)：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mn>3</mn> </msub> <msubsup> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>3</mn> </msubsup> <mo>+</mo> <msub> <mi>a</mi> <mn>2</mn> </msub> <msubsup> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <msub> <mi>a</mi> <mn>1</mn> </msub> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>a</mi> <mn>0</mn> </msub> <mo>=</mo> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>3</mn> <msub> <mi>a</mi> <mn>3</mn> </msub> <msubsup> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <mn>2</mn> <msub> <mi>a</mi> <mn>2</mn> </msub> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>a</mi> <mn>1</mn> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mn>3</mn> </msub> <msubsup> <mi>t</mi> <mi>i</mi> <mn>3</mn> </msubsup> <mo>+</mo> <msub> <mi>a</mi> <mn>2</mn> </msub> <msubsup> <mi>t</mi> <mi>i</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msub> <mi>a</mi> <mn>1</mn> </msub> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>a</mi> <mn>0</mn> </msub> <mo>=</mo> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>3</mn> <msub> <mi>a</mi> <mn>3</mn> </msub> <msubsup> <mi>t</mi> <mi>i</mi> <mn>2</mn> </msubsup> <mo>+</mo> <mn>2</mn> <msub> <mi>a</mi> <mn>2</mn> </msub> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>a</mi> <mn>1</mn> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow>
4.根据权利要求1所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，步骤3.2)中机械臂对基座产生的合力包括惯性力、流体力、浮力和重力；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力包括线缆力、流体力、重力和浮力。
5.根据权利要求1所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，第四步对重新标定的机械臂轨迹进行求解包括以下步骤：
<mrow> <msub> <mi>q</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mover> <mi>q</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mo>(</mo> <mi>r</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，r(t)是使用轨迹动态标度形成的时间规律函数，q(t)是机械臂规划求出的机械臂关节运动轨迹；对于定标函数，选择简单的线性函数形式：
R(t)＝ct；
其中c是一个大于1的常数，且r(t)＝0,
对公式进行两次求导，得到关系式：
机械臂动力学方程中由速度和加速度产生的项可以表示成：
<mrow> <msub> <mi>&amp;tau;</mi> <mi>s</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>C</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>D</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
将相关公式代入，能够得到函数r(t)重新标定过的机械臂动力学方程为：
<mrow> <msub> <mi>&amp;tau;</mi> <mi>s</mi> </msub> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>c</mi> <mn>2</mn> </msup> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>C</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>,</mo> <mi>c</mi> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mo>)</mo> </mrow> <mi>c</mi> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>D</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mo>,</mo> <mi>c</mi> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mo>)</mo> </mrow> <mi>c</mi> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
把其中后两项线性化得到关系式：
<mrow> <msub> <mi>&amp;tau;</mi> <mi>s</mi> </msub> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>c</mi> <mn>2</mn> </msup> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mi>c</mi> <mn>2</mn> </msup> <mi>C</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mi>c</mi> <mn>2</mn> </msup> <mi>D</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
公式变为：
6.根据权利要求1或5所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，机械臂动力学方程中与速度和加速度有关的项以c2的比例被减小或增大。
CN201710237568.9A 2017-04-12 2017-04-12 中性浮力实验中漂浮基座与机械臂的动态标度规划方法 Active CN107145640B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710237568.9A CN107145640B (zh) 2017-04-12 2017-04-12 中性浮力实验中漂浮基座与机械臂的动态标度规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710237568.9A CN107145640B (zh) 2017-04-12 2017-04-12 中性浮力实验中漂浮基座与机械臂的动态标度规划方法

Publications (2)

Publication Number Publication Date
CN107145640A true CN107145640A (zh) 2017-09-08
CN107145640B CN107145640B (zh) 2020-11-06

Family

ID=59774653

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710237568.9A Active CN107145640B (zh) 2017-04-12 2017-04-12 中性浮力实验中漂浮基座与机械臂的动态标度规划方法

Country Status (1)

Country Link
CN (1) CN107145640B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107696033A (zh) * 2017-09-18 2018-02-16 北京控制工程研究所 一种基于视觉测量的空间机械臂轨迹滚动规划方法
CN110561433A (zh) * 2019-08-31 2019-12-13 中山大学 一种提高机械臂运动规划控制精度的方法
CN112743574A (zh) * 2020-12-28 2021-05-04 深圳市优必选科技股份有限公司 机械臂设计的优化方法、装置和设备
CN114536348A (zh) * 2022-04-08 2022-05-27 北京邮电大学 一种高欠驱动空间机械臂运动灵巧性评估方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104590593A (zh) * 2015-01-16 2015-05-06 西北工业大学 航天器地面微重力实验中心引力标定方法
CN105138000A (zh) * 2015-08-06 2015-12-09 大连大学 最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104590593A (zh) * 2015-01-16 2015-05-06 西北工业大学 航天器地面微重力实验中心引力标定方法
CN105138000A (zh) * 2015-08-06 2015-12-09 大连大学 最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
JIANPING YUAN等: "An innovative method for simulating microgravity effects through combining electromagnetic force and buoyancy", 《ADVANCES IN SPACE RESEARCH》 *
ZHANXIA ZHU等: "An improving method for micro-G simulation with magnetism-buoyancy hybrid system", 《ADVANCES IN SPACE RESEARCH》 *
陈诗瑜等: "一种有效的中性浮力下实验体姿态机动模拟控制律设计", 《西北工业大学学报》 *
陈诗瑜等: "中性浮力下实验体模拟对地定向控制律设计", 《科学技术与工程》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107696033A (zh) * 2017-09-18 2018-02-16 北京控制工程研究所 一种基于视觉测量的空间机械臂轨迹滚动规划方法
CN110561433A (zh) * 2019-08-31 2019-12-13 中山大学 一种提高机械臂运动规划控制精度的方法
CN110561433B (zh) * 2019-08-31 2022-07-19 中山大学 一种提高机械臂运动规划控制精度的方法
CN112743574A (zh) * 2020-12-28 2021-05-04 深圳市优必选科技股份有限公司 机械臂设计的优化方法、装置和设备
CN112743574B (zh) * 2020-12-28 2022-07-19 深圳市优必选科技股份有限公司 机械臂设计的优化方法、装置和设备
CN114536348A (zh) * 2022-04-08 2022-05-27 北京邮电大学 一种高欠驱动空间机械臂运动灵巧性评估方法
CN114536348B (zh) * 2022-04-08 2023-05-26 北京邮电大学 一种高欠驱动空间机械臂运动灵巧性评估方法

Also Published As

Publication number Publication date
CN107145640B (zh) 2020-11-06

Similar Documents

Publication Publication Date Title
CN107145640A (zh) 中性浮力实验中漂浮基座与机械臂的动态标度规划方法
Jin et al. Six-degree-of-freedom hovering control of an underwater robotic platform with four tilting thrusters via selective switching control
Qiao et al. Design of three exponentially convergent robust controllers for the trajectory tracking of autonomous underwater vehicles
Chen et al. Recurrent neural network-based robust nonsingular sliding mode control with input saturation for a non-holonomic spherical robot
CN104317300B (zh) 一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法
Rodic et al. Control of a Quadrotor Flight
CN106393116B (zh) 具有初态学习的机械臂分数阶迭代学习控制方法及系统
CN108829099A (zh) 一种基于受限反步法控制的欠驱动无人船轨迹跟踪方法
CN107272677A (zh) 一种移动机器人的变结构自适应轨迹跟踪控制方法
CN106292294A (zh) 基于模型参考自适应控制的舰载无人机自动着舰控制装置
CN106708068A (zh) 仿生波动鳍推进水下航行器路径跟踪控制方法
CN104199303B (zh) 一种基于矢量场制导的平流层卫星平面路径跟踪控制方法
Jagtap et al. Control of autonomous underwater vehicle using reduced order model predictive control in three dimensional space
Fahimi et al. Alternative trajectory-tracking control approach for marine surface vessels with experimental verification
CN103576689A (zh) 一种交会对接六自由度相对控制方法
Patel et al. An intelligent hybrid artificial neural network-based approach for control of aerial robots
CN107450318A (zh) 一种基于二阶滑模控制的气垫船路径跟踪控制方法
CN107608368B (zh) 一种无人机在极端初始状态的快速平衡控制方法
CN105182990A (zh) 具有输出受限的三自由度模型直升机的鲁棒控制方法
Cheng et al. Robust three-dimensional path-following control for an under-actuated stratospheric airship
CN104965515A (zh) 一种基于自适应控制的螺旋桨无人靶机的姿态控制方法
CN107977011A (zh) 基于分数阶控制算法的四旋翼无人机飞行控制方法
CN107817818A (zh) 一种模型不确定飞艇航迹跟踪有限时间控制方法
CN108829109B (zh) 基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法
Wong et al. Model linearization and H∞, controller design for a quadrotor unmanned air vehicle: Simulation study

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