[发明专利]一种机械臂多点运动轨迹规划方法

权利要求书

1.一种机械臂多点运动轨迹规划方法，其特征在于，包括以下步骤：

A.设置机械臂运动参数；

B.利用克莱姆法则，生成多元齐次线性方程，生成并计算机械臂轨迹生成的自适应多项式θ(t)；

C.将步骤B得到的轨迹曲线对时间进行一阶、二阶求导，得到机械臂各连接关节速度v(t)、加速度a(t)曲线；

D.利用牛顿-欧拉逆动力学计算机械臂各连接关节的力矩τ(t)；

E.通过力位混合控制器控制机械臂完成规定运动，并反馈机械臂各连接关节当前位置与速度，重复进行步骤A-E直至机械臂完成所生成的运动轨迹。

2.根据权利要求1所述的一种机械臂多点运动轨迹规划方法，其特征在于，所述步骤A包括以下步骤：

A1.设置机械臂各连接关节的初始角度θ₀与终止角度θ_f，初始时间t₀与终止时间t_f；

A2.机械臂在初始和终止时处于静止状态，设置机械臂在初始时间t₀与终止时间t_f时的初始速度v₀和终止速度v_f为0；

A3.设置n个路径点的关节空间参数，包括各个路径点的时间信息t₁～t_n与位置信息θ₁～θ_n。

3.根据权利要求1所述的一种机械臂多点运动轨迹规划方法，其特征在于，所述步骤B包括以下步骤：

B1.确定边界条件数量；根据步骤A得到当时间为[t₀，t₁，…，t_n，t_f]时，角度位置为[θ₀，θ₁，…，θ_n，θ_f]，其位置信息共有n+2个边界条件；当时间为[t₀，t_f]时，速度[v₀，v_f]＝0，其速度信息共有2个边界条件；

B2.根据史莱姆法则，若需要拟合其运动轨迹，则需要生成n+4元线性方程进行线性求解，得到生成对应n+3元自适应多项式轨迹：

B3.根据步骤B1与列出n+4元齐次线性方程组为：

B4.将步骤B3得到的n+4元齐次线性方程组写成矩阵模式为：

T_i，ja_i＝b_j；

其中T_i，j为大小为(n+4)×(n+4)的矩阵，具体为关于各约束条件的时间信息：

b_j为大小为(n+4)×1的矩阵，具体为关于各约束条件的位置及速度信息：

B5.求得a_i为大小为(n+4)×1的矩阵，具体为关于轨迹生成的自适应

多项式各项系数：

B6.重新整理得到机械臂多点约束条件下的运动轨迹：

4.根据权利要求1所述的一种机械臂多点运动轨迹规划方法，其特征在于，所述步骤D包括以下步骤：

D1.设置机械臂各连杆中心位置及姿态矩阵M，各连杆转动惯量矩阵G，各关节旋转轴矩阵S，重力场向量g；

D2.通过D1确定的动力学模型，利用牛顿-欧拉逆动力学公式得到机械臂各关节的力矩信息：

τ(t)＝M(θ(t))a(t)+c(θ(t)，v(t))+g(θ(t))+F_tip

其中，M(θ(t))为机械臂各关节的质量矩阵，c(θ(t)，v(t))为机械臂转动所产生的科里奥利力，g(θ(t))为机械臂在重力场下产生的重力，F_tip为机械臂末端的负载；

质量矩阵M(θ(t))与重力g(θ(t))在关节空间中表示为关于机械臂各关节角度的函数，科里奥利力c(θ(t)，v(t))在关节空间表示为关于机械臂各关节角度和速度的函数。

5.根据权利要求1所述的一种机械臂多点运动轨迹规划方法，其特征在于：所述步骤E包括以下步骤：

E1.将步骤B6得到的位置曲线θ(t)与步骤D2得到的力矩曲线τ(t)作为输入，通过力位混合控制器对机械臂各关节电机进行驱动；

E2.机械臂各关节电机通过磁编码器反馈各关节当前的位置信息θ_new与速度信息v_new，通过计时器反馈运动过程所用时间t_e；

E3.返回步骤A1更新机械臂各关节初始角度与速度信息：

当t＝t₀时其初始角度θ₀＝θ_new，其初始速度v₀＝v_new；

此时机械臂的总体运动时间为t＝t_f-t_e，机械臂各关节终止角度与速度参数不变；

返回步骤A2设置其中间路径点参数信息：

定义n个路径点的关节空间参数，包括各个路径点的时间信息(t₁～t_n)-t_e与位置信息θ₁～θ_n；

当运动过程所用的时间t_e＞t₁时，其中间路径点数量减一：n＝n-1；

循环进行步骤A-E直至t_e＝t_f或者θ_new＝θ_f，此时机械臂即完成了多路径点约束的轨迹规划。