[发明专利]一种冗余度机械臂突加度层运动规划的初始化方法

权利要求书

1.一种冗余度机械臂突加度层运动规划的初始化方法，其特征在于，包括：

S1、启动机械臂控制器，通过编码器读取机械臂各个关节当前的角度；

S2、将所述各个关节当前的角度作为起始值；将机械臂执行运动规划任务时的关节角度作为期望值；基于所述起始值和期望值，在突加度层上建立二次型优化方案；

S3、将所述二次型优化方案转换为分段线性投影方程，并采用数值算法对所述分段线性投影方程进行求解，得到最优解；

S4、所述机械臂控制器根据所述最优解，驱动机械臂的各个关节，使其从当前位置调整到所述机械臂执行运动规划任务时的位置。

2.如权利要求1所述的一种冗余度机械臂突加度层运动规划的初始化方法，其特征在于，所述S2中二次型优化方案的性能指标表示为：

最小化

其中，θ∈Rⁿ表示机械臂关节角度；表示机械臂关节速度；表示机械臂关节加速度；表示机械臂关节突加度；θ_d∈Rⁿ表示机械臂执行运动规划任务时的起始关节角度；T表示矩阵或向量的转置；参数λ＞0∈R。

3.如权利要求2所述的一种冗余度机械臂突加度层运动规划的初始化方法，其特征在于，所述二次型优化方案的约束条件包括：

θ^-≤θ≤θ⁺ (2)

其中，θ^-表示机械臂关节角度的下限；θ⁺表示机械臂关节角度的上限；表示机械臂关节速度的下限；表示机械臂关节速度的上限；表示机械臂关节加速度的下限；表示机械臂关节加速度的上限；表示机械臂关节突加度的下限；表示机械臂关节突加度的上限。

4.如权利要求3所述的一种冗余度机械臂突加度层运动规划的初始化方法，其特征在于，

将公式(2)-(5)转化为关于机械臂关节突加度的双端约束，表示为：

其中，ξ^-和ξ⁺分别表示的下限和上限；所述ξ^-和ξ⁺的第i个元素分别定义为：

其中，α表示机械臂关节角度对应的转换参数，α＞0∈R；β表示机械臂关节速度对应的转换参数，β＞0∈R；γ表示机械臂关节加速度对应的转换参数，γ＞0∈R；δ表示机械臂关节角度极限的转换裕量，δ＞0∈R；η表示机械臂关节速度极限的转换裕量η＞0∈R；下标i＝1,2,…,n。