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

Abstract

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

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个三阶多项式方程，将每段轨迹通过逆运动学方程计算成相应的关节变量；

第三步、求解标度因子；

3.1)求解机械臂的期望轨迹，根据任务要求确定基座的期望位置、期望姿态以及期望运动状态；

3.2)将基座和机械臂的期望运动参数代入递归算法进行动力学逆解，确定轨迹；机械臂对基座产生的合力以及合力矩表示为τ_s(t)；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力及合力矩表示为τ_w(t)；

3.3)把τ_s(t)和τ_w(t)通过公式分配给各个推进器，分别表示为τ_spk(t)和τ_wpk(t)，其中k＝1,2,.....,6，求解分配到推进器k的|τ_spk(t)|在时间t_i-1和t_i之间的最大值|τ_{spk max}(t_km)|，τ_wpk(t)在此时刻的值为τ_wpk(t_km)；

3.4)已知每个理想推进器的最大推力均为τ_{p max}，每个推进器的标度因子c(k)为：

3.5)选择六个标度因子的最大值max(c(k))作为机械臂期望轨迹的标度因子c_i；

第四步、对重新标定的机械臂轨迹进行求解，在每段关节空间轨迹中通过相同的规则分别求得轨迹动态标度，形成多个新的轨迹，路径相同而时间规律不同的运动轨迹表达式如下：

第一步轨迹路径的参数表达式为：P＝f(s)；其中，s为路径长度，P为末端执行器在水池坐标系中的坐标；当t＝0时s＝0，当t＝t_f时s＝s_f。

第二步n个三阶多项式方程定义为Π_i(t)，i＝1,2,…,n，通用约束条件为：

Π_i(t_i-1)＝q_ei-1；

Π_i(t_i)＝q_ei；

对应速度根据下列规则进行计算：

其中，

给出时间区间[t_k-1,t_k]内分段的斜率；

选择三次多项式：q(t)＝a₃t³+a₂t²+a₁t+a₀

得到抛物线的速度曲线：

通过求解如下方程，得到关节变量：

步骤3.2)中机械臂对基座产生的合力包括惯性力、流体力、浮力和重力；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力包括线缆力、流体力、重力和浮力。

第四步对重新标定的机械臂轨迹进行求解包括以下步骤：

其中，r(t)是使用轨迹动态标度形成的时间规律函数，q(t)是机械臂规划求出的机械臂关节运动轨迹；对于标定函数，选择简单的线性函数形式：

R(t)＝ct；

其中c是一个大于1的常数，且r(t)＝0,

对公式进行两次求导，得到关系式：

机械臂动力学方程中由速度和加速度产生的项可以表示成：

将相关公式代入，能够得到函数r(t)重新标定过的机械臂动力学方程为：

把其中后两项线性化得到关系式：

公式变为：

机械臂动力学方程中与速度和加速度有关的项以c²的比例被减小或增大。

与现有技术相比，本发明具有如下的有益效果：某些任务要求下，机械臂的轨迹是建立在基座始终保持在某一位姿这一假设上的。基座的位姿稳定是依靠基座本身的静稳定性和螺旋桨推进器输出的推力来维持，而如果基座稳定所需的力和力矩大于螺旋桨推进器所能提供的最大推力和最大推力矩，那么基座就无法保持位姿的稳定。本发明通过对运动轨迹的动态时间标定，形成新的机械臂关节角速度和角加速度，从而改变机械臂的惯性力和环境力，使得原先无法保持基座位姿稳定的规划方法实现基座位姿的稳定。通过仿真实验证实，本发明能够使基座三个自由度的位置误差及姿态误差始终满足要求，实用性以及可实施性较强。

附图说明

图1本发明规划方法的整体流程图；

图2(a)-(b) 未进行动态标度的仿真实验结果数据统计图：

图 2 (a)基座位置数据图；图2(b)基座姿态数据图；

图3(a)-(b) 未进行动态标度的仿真实验中推进器推力数据图：

图 3 (a)1-3号推进器推力数据图；图3(b)4-6号推进器推力数据图；

图4(a)-(b) 使用动态标度的仿真实验结果数据统计图：

图 4 (a)Oxy平面轨迹示意图；图4(b)Oxz平面轨迹示意图；

图5(a)-(b) 使用动态标度的仿真实验结果数据统计图：

图 5 (a)关节角数据统计图；图5(b)关节角速度数据统计图；

图6(a)-(b) 使用动态标度的仿真实验结果数据统计图：

图 6 (a)基座位置数据图；图6(b)基座姿态数据图；

图7(a)-(b) 使用动态标度的仿真实验结果数据统计图：

图 7 (a)1-3号推进器推力数据图；图7(b)4-6号推进器推力数据图。

具体实施方式

下面结合附图对本发明做进一步的详细说明。

参见图1，本发明动态标度规划方法包括以下步骤：

第一步、在水池坐标系中设定机械臂末端执行器的轨迹，并在轨迹内***一个路径点序列，进而将轨迹划分为n段执行；

路径的参数表达式为：P＝f(s)；其中，s为路径长度，P为末端执行器在水池坐标系中的坐标；当t＝0时s＝0，当t＝t_f时s＝s_f。

第二步、在n段轨迹上建立n个三阶多项式方程，将每段轨迹通过逆运动学方程计算成相应的关节变量；

n个三阶多项式方程定义为Π_i(t)，i＝1,2,…,n，通用约束条件为：

Π_i(t_i-1)＝q_ei-1；

Π_i(t_i)＝q_ei；

对应速度根据下列规则进行计算：

其中，

给出时间区间[t_k-1,t_k]内分段的斜率；

选择三次多项式：q(t)＝a₃t³+a₂t²+a₁t+a₀

得到抛物线的速度曲线：

通过求解如下方程，得到关节变量：

第三步、求解标度因子；

3.1)求解机械臂的期望轨迹，根据任务要求确定基座的期望位置、期望姿态以及期望运动状态；

3.2)将基座和机械臂的期望运动参数代入递归算法进行动力学逆解，确定轨迹；机械臂对基座产生的惯性力、流体力、浮力和重力的合力以及合力矩表示为τ_s(t)；基座在水下期望位置、期望姿态以及期望运动状态下受到的线缆力、流体力、重力和浮力的合力及合力矩表示为τ_w(t)；

3.3)把τ_s(t)和τ_w(t)通过公式分配给各个推进器，分别表示为τ_spk(t)和τ_wpk(t)，其中k＝1,2,.....,6，求解分配到推进器k的|τ_spk(t)|在时间t_i-1和t_i之间的最大值|τ_{spk max}(t_km)|，τ_wpk(t)在此时刻的值为τ_wpk(t_km)；

3.4)已知每个理想推进器的最大推力均为τ_{p max}，每个推进器的标度因子c(k)为：

3.5)选择六个标度因子的最大值max(c(k))作为机械臂期望轨迹的标度因子c_i；

第四步、对重新标定的机械臂轨迹进行求解，在每段关节空间轨迹中通过相同的规则分别求得轨迹动态标度，形成多个新的轨迹：

其中，r(t)是使用轨迹动态标度形成的时间规律函数，q(t)是机械臂规划求出的机械臂关节运动轨迹；对于标定函数，选择简单的线性函数形式：

R(t)＝ct；

其中c是一个大于1的常数，且r(t)＝0,

对公式进行两次求导，得到关系式：

机械臂动力学方程中由速度和加速度产生的项可以表示成：

将相关公式代入，能够得到函数r(t)重新标定过的机械臂动力学方程为：

把其中后两项线性化得到关系式：

公式变为：

上式给出了路径相同而时间规律不同的运动轨迹表达式。机械臂动力学方程中与速度和加速度有关的项以c²的比例被减小或增大。

实施例

在这组仿真实验中设置基座的初始位置坐标为[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控制器给关节角输出控制信号，控制器增益系数K_p设为diag(300,200)，微分增益系数K_d设置为diag(70,50)，积分增益系数K_i设为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个三阶多项式方程，将每段轨迹通过逆运动学方程计算成相应的关节变量；

第三步、求解标度因子；

3.1)求解机械臂的期望轨迹，根据任务要求确定基座的期望位置、期望姿态以及期望运动状态；

3.2)将基座和机械臂的期望运动参数代入递归算法进行动力学逆解，确定轨迹；机械臂对基座产生的合力以及合力矩表示为τ_s(t)；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力及合力矩表示为τ_w(t)；

3.3)把τ_s(t)和τ_w(t)通过公式分配给各个推进器，分别表示为τ_spk(t)和τ_wpk(t)，其中k＝1,2,.....,6，求解分配到推进器k的|τ_spk(t)|在时间t_i-1和t_i之间的最大值|τ_spkmax(t_km)|，τ_wpk(t)在此时刻的值为τ_wpk(t_km)；

3.4)已知每个理想推进器的最大推力均为τ_pmax，每个推进器的标度因子c(k)为：

3.5)选择六个标度因子的最大值max(c(k))作为机械臂期望轨迹的标度因子c_i；

第四步、对重新标定的机械臂轨迹进行求解，在每段关节空间轨迹中通过相同的规则分别求得轨迹动态标度，形成多个新的轨迹，路径相同而时间规律不同的运动轨迹表达式如下：

2.根据权利要求1所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，第一步轨迹路径的参数表达式为：P＝f(s)；其中，s为路径长度，P为末端执行器在水池坐标系中的坐标；当t＝0时s＝0，当t＝t_f时s＝s_f。

3.根据权利要求1所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，第二步n个三阶多项式方程定义为Π_i(t)，i＝1,2,…,n，通用约束条件为：

Π_i(t_i-1)＝q_ei-1；

Π_i(t_i)＝q_ei；

对应速度根据下列规则进行计算：

其中，

给出时间区间[t_k-1,t_k]内分段的斜率；

选择三次多项式：q(t)＝a₃t³+a₂t²+a₁t+a₀

得到抛物线的速度曲线：

通过求解如下方程，得到关节变量：

4.根据权利要求1所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，步骤3.2)中机械臂对基座产生的合力包括惯性力、流体力、浮力和重力；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力包括线缆力、流体力、重力和浮力。

5.根据权利要求1所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，第四步对重新标定的机械臂轨迹进行求解包括以下步骤：

其中，r(t)是使用轨迹动态标度形成的时间规律函数，q(t)是机械臂规划求出的机械臂关节运动轨迹；对于标定函数，选择简单的线性函数形式：

R(t)＝ct；

其中c是一个大于1的常数，且r(t)＝0,

对公式进行两次求导，得到关系式：

机械臂动力学方程中由速度和加速度产生的项可以表示成：

将相关公式代入，能够得到函数r(t)重新标定过的机械臂动力学方程为：

把其中后两项线性化得到关系式：

公式变为：

6.根据权利要求1或5所述中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于，机械臂动力学方程中与速度和加速度有关的项以c²的比例被减小或增大。

Images