[发明专利]一种机器人轨迹同步控制方法、计算机可读存储介质

权利要求书

1.一种机器人轨迹同步控制方法，其特征在于，包括以下步骤：

步骤1.采用质量弹簧阻尼模型的二阶系统对机器人末端的位置和姿态即位姿进行闭环控制，得到笛卡尔空间下的线加速度和角加速度

步骤2.将笛卡尔空间下的线加速度和角加速度转换成机器人的关节速度和关节加速度

步骤3.若通过步骤1和步骤2得到的机器人末端的位姿及关节速度与设定值存在偏差时，引入内环控制回路，控制机器人各个关节按照关节加速度进行加减速，以补偿机器人各个关节运动时对其他关节产生的耦合力矩；

步骤4.在机器人每个控制插补周期内，根据机器人末端的设定位姿与实际位姿之间的偏差，计算每个关节的关节加速度并作用于机器人的内环控制回路的控制器上，使设定位姿与实际位姿之间的偏差趋于0以实现机器人轨迹同步。

2.根据权利要求1所述的一种机器人轨迹同步控制方法，其特征在于，所述步骤1具体包括：

步骤1.1.根据机器人各个关节的反馈角度q，采用机器人正运动公式计算机器人末端的反馈位姿X，具体包括：

步骤1.1.1.采用标准D-H法建立机器人的运动学模型，得到机器人的D-H参数：d、a、α；

步骤1.1.2.根据坐标变换方法得到机器人末端坐标系{n}在机器人极坐标系下的齐次变换矩阵：

其中，n表示机器人关节的数目，表示从第i-1个关节的坐标系{i-1}到第i个关节的坐标系{i}的齐次变换矩阵，i＝1,2,3...n，且的计算公式如下：

则X可表示为：

其中，表示矩阵的第i行的第j列的元素；

步骤1.2.根据用户指定的期望位姿矩阵计算得到期望位姿X_d；

步骤1.3.计算期望位姿X_d与反馈位姿X之间的位姿误差e；

步骤1.4.采用二阶系统对末端位姿进行闭环控制，计算得到笛卡尔空间下的线加速度和角加速度

3.根据权利要求2所述的一种机器人轨迹同步控制方法，其特征在于，所述步骤1.2中期望位姿X_d具体计算如下：

其中

4.根据权利要求3所述的一种机器人轨迹同步控制方法，其特征在于，所述步骤1.3中位姿误差e的计算方法如下：

其中，

5.根据权利要求4所述的一种机器人轨迹同步控制方法，其特征在于，所述步骤1.4中采用的二阶系统可等效为质量弹簧阻尼模型。

6.根据权利要求5所述的一种机器人轨迹同步控制方法，其特征在于，笛卡尔空间下的线加速度和角加速度的计算方法如下：

位姿误差e满足如下方程：

其中，K_d和K_p均为6×6的正定矩阵，

其中v_d、表示设定的线速度和设定的线加速度，w_d、表示设定的角速度和设定的角加速度，v、表示线速度和线加速度，其中w、表示角速度和角加速度，代入式(8)可得：

7.根据权利要求6所述的一种机器人轨迹同步控制方法，其特征在于，所述步骤2具体为：

根据机器人正向速度关系知：

其中，J(q)表示机器人的雅克比矩阵，表示机器人的关节速度；

将式(10)左右两端同时求导数得：

其中，表示雅克比矩阵的导数，则对于机器人不经过奇异点时有：

将步骤1.4中已经求得的代入式(12)得到：