[发明专利]中性浮力实验中漂浮基座与机械臂的动态标度规划方法

权利要求书

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)中机械臂对基座产生的合力包括惯性力、流体力、浮力和重力；基座在水下期望位置、期望姿态以及期望运动状态下受到的合力包括线缆力、流体力、重力和浮力。