[发明专利]一种基于急突度的机械臂最小运动规划和控制方法

权利要求书

1.一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，包括以下步骤：

S1：设定机械臂最小运动规划的轨迹以及物理限制；

S2：根据所设定的轨迹以及物理限制，将机械臂的最小运动规划和控制问题转化为急突度层二次规划问题；

S3：对急突度层二次规划问题进行求解，机械臂根据求解结果进行最小运动。

2.根据权利要求1所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，所述步骤S1中设定的机械臂最小运动规划的轨迹为：

r_d(t)

其中，r_d(t)为机械臂在t时刻末端执行器的期望坐标。

3.根据权利要求1所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，所述步骤S1中设定的物理限制为：

θ^-≤θ(t)≤θ⁺

其中，θ(t)为机械臂各关节在t时刻的角度向量，θ⁺为角度向量上限，θ^-为角度向量下限，为t时刻的角速度向量，为角速度向量上限，为角速度向量下限，为t时刻的角加速度向量，为角加速度向量上限，为角加速度下限，为t时刻的角突加度向量，为角突加度上限，为角突加度下限，为t时刻的角急突度向量，为角急突度上限，为角急突度下限。

4.根据权利要求1所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，所述步骤S2具体为：

S201：将轨迹以及物理限制转化到急突度层，得到急突度层约束，并通过在急突度层设定机械臂末端执行器回到初始点完成最小运动，得到最小化目标；

S202：将急突度层约束和最小化目标组成急突度层二次规划问题。

5.根据权利要求4所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，所述步骤S201中获取的急突度层约束为：

其中，J为末端执行器的雅可比矩阵，

为J的时间导数，为的时间导数，为的时间导数，为r_d(t)的时间导数，为的时间导数，为的时间导数，为的时间导数，f(θ(t))为根据角向量得到的t时刻末端执行器的实际坐标，收敛参数γ0。

6.根据权利要求4所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，所述步骤S201中获取的最小化目标为：

其中，Z为最小化目标；上标T为向量转置标志。

7.根据权利要求1所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，所述步骤S3具体为：

S301：对急突度层二次规划问题进行求解；

S302：将求解结果转化为机械臂动力源的驱动信号；

S303：机械臂动力源接收到驱动信号后，驱动机械臂进行最小运动。

8.根据权利要求7所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，在所述步骤S301中，采用二次规划求解器对急突度层二次规划问题进行求解。

9.根据权利要求7所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，在所述步骤S302中，采用控制器将求解结果转化为机械臂动力源的驱动信号。

10.根据权利要求7所述的一种基于急突度的机械臂最小运动规划和控制方法，其特征在于，在所述步骤S303中，当机械臂所进行的最小运动结束后，获取机械臂末端的运动轨迹。