一种六自由度机械臂考虑力约束的轨迹规划方法与流程

技术特征：

1.一种六自由度机械臂考虑力约束的轨迹规划方法，其特征在于，包括如下步骤：

(1)将力约束转化为接触形变和接触运动速度的约束；

根据接触模型将机械臂末端与环境的接触分为静态接触和动态接触；

(1.1)静态接触过程中，将力的约束转化为沿法线方向的接触位移的约束，即

$X_E=\sqrt{F/K_s}\≤\sqrt{F_{\max }/K_s}=X_{E\max }$

其中，K_s是接触刚度；X_E是机械臂末端沿接触面法向的形变量；F表示机械末端力传感器测量的接触面法线方向上的接触力；

(1.2)动态接触也就是冲击，机械臂末端与外界环境的接触碰撞过程在极短的时间内完成，将力约束转化为接触速度的约束，来实现满足机械臂轨迹规划过程中冲击最优；

(2)优化动态接触冲击的机械臂运动速度；

(2.1)采用S型加减速曲线使加速度连续；

采用S型速度变化使得运动过程中加速度连续，减小各关节的冲击，使机械臂运动和作业满足力的约束；

优化后的S型速度曲线共分为七个过程：

(2.1.1)加加速度一定、加速度匀速增加过程，加速度从初始值加速到最大加速度；

(2.1.2)匀加加速度加速过程，保持最大加速度不变，速度匀速增加；

(2.1.3)减加速度加速过程，速度继续增加，加速度以恒定的加加速度减小直到速度加到最大速度值；

(2.1.4)匀速运动过程，保持最大速度运动，加速度为零；

(2.1.5)减减速度减速过程，速度减小，减速度以恒定的减减速度减小；

(2.1.6)匀减减速度减速过程，保持减速度不变，速度匀速减小；

(2.1.7)减减速度一定减速度匀速减小过程；

(2.2)归一化时间算子；

笛卡尔空间位置插补的表达式为：

P(t)＝P_s+l(t)(P_e-P_s)，

其中，P(t)表示插补点的坐标；l(t)为归一化时间算子；P_s为期望位置；P_e为初始位置；

要对时间算子进行平滑处理，使得速度曲线平滑，定义归一化时间算子的表达式为：

$l\left(t\right)=\frac{S\left(t_{i+1}\right)-S\left(t_i\right)}{L},$

其中，i＝0,1,2,3,4,5,6；l(0)＝0；l(T)＝1；l(t)在区间内单调递增；S(t)为各时间段内对应的总位移；

(3)基于力约束进行轨迹规划；

(3.1)基于规则接触边缘具有力约束的连续轨迹规划：

规则接触边缘具有力约束的连续轨迹采用圆弧过渡的方法代替直线-直线连续轨迹规划方法进行平滑处理，达到运动在接触临界接触面时不具有沿接触面法线方向的位移、速度和加速度分量，过渡圆弧的联结点分别选取在前一段直线的减速段和后一段直线的加速段,；

(3.1.1)在接触模型的有效的情况下，得到机械臂末端与外界环境的接触刚度为K，在已知约束力为F的情况下，可求得接触形变X_e：

X_e＝F/K；

(3.1.2)通过接触形变，得到临界接触点B点的空间位置坐标p₂：

p₂＝p_q+X_e；

轨迹终点M的空间位置坐标p₃为：

p₃＝p₂+L；

(3.1.3)过渡精度的确定，过渡精度决定过渡圆弧的大小，决定实际轨迹与期望轨迹逼近程度；设过渡力大小为F'，则过渡精度re为：

re＝F-F'

当力传感器示数时，进行空间直线轨迹运动；当力传感器示数时，为过渡圆弧的起点；当力传感器示数时，进行圆弧过渡运运动；当力传感器示数时，为过渡圆弧的终点，之后机械臂末端与外界环境保持力F进行沿接触面切平面方向的运动；

(3.1.4)确定过渡圆弧起始联结点p_e1和过渡圆弧终止联结点p_e2；

当力传感器示数到达过渡力F'时开始圆弧过渡运动，当力传感器示数到达约束力F时结束圆弧过渡运动；即过渡圆弧联结点为接触力为F'和F时机械臂末端的位置点；

当力传感器示数时，通过位置编码器可知当前机械臂六个关节关节角θ_e11～θ_e16，通过机械臂正运动学式得到此刻对应的机械臂末端实际位置即为过渡圆弧起始联结点p_e1；当力传感器示数时，通过位置编码器可知当前机械臂六个关节关节角θ_e21～θ_e26，通过机械臂正运动学式得到此刻对应的机械臂末端实际位置即为过渡圆弧终止联结点p_e2；

(3.1.5)轨迹生成器通过路径点和过渡圆弧起始联结点p_e1和终止联结点p_e2的空间位置坐标，采用具有S型速度曲线拟合的空间直线和圆弧规划方法生成运动轨迹；

(3.1.6)运动过程中用力传感器进行监测，看整个运动过程是否满足期望力F的约束，如果时，重新规划。

(3.2)基于曲率接触边缘具有力约束的连续轨迹规划：

(3.2.1)机械臂实现沿接触面法线方向直线运动和沿临界安全接触面的接触运动和沿接触面法线方向远离临界安全接触面的直线运动，是一个直线-圆弧-直线连续的轨迹规划过程，确定运动过程中在安全接触圆柱面上的具有力约束运动中的特殊点M、N点；

(3.2.2)采用圆弧过渡来解决直线-圆弧连续运动时产生的尖角问题；使得末端在沿安全接触面的运动过程中只有沿接触面切线方向位移和速度，法线方向速度为零；

(3.2.3)通过调节过渡精度来调整实际轨迹与期望轨迹的精度，满足接触模型的基于圆柱接触面的连续轨迹规划方法与基于规则接触面连续轨迹规划方法中过渡圆弧联结点求取方法一致，过渡完后机械臂末端是在一定接触力下沿着接触面的圆弧轨迹。