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

摘要

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

主权项

一种中性浮力实验中漂浮基座与机械臂的动态标度规划方法，其特征在于：第一步、在水池坐标系中设定机械臂末端执行器的轨迹，并在轨迹内插入一个路径点序列，进而将轨迹划分为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)为：ci(k){=|τspkmax(tkm)|τpmax-|τpk(tkm)|=|τspkmax(tkm)|τpmax+|τpk(tkm)|,sgn(τspkmax(tkm))=sgn(τwpk(tkm))sgn(τspkmax(tkm))≠sgn(τwpk(tkm));]]>3.5)选择六个标度因子的最大值max(ci(k))作为机械臂期望轨迹的标度因子ci；第四步、对重新标定的机械臂轨迹进行求解，在每段关节空间轨迹中通过相同的规则分别求得轨迹动态标度，形成多个新的轨迹，路径相同而时间规律不同的运动轨迹表达式如下：τs(r)=c2τs‾(ct).]]>