面向机器人末端位姿补偿的全闭环反馈控制方法及系统

本发明属于机器人控制相关，更具体地，涉及一种面向机器人末端位姿补偿的全闭环反馈控制方法及系统。

背景技术：

1、传统机器人控制方法通常采用开环控制或简单的闭环控制，难以适应复杂和变化的工作环境。在简单闭环控制中，pid控制器被广泛应用，但在实际应用场景中pid参数调整仍采用手动调参或z-n法。

2、中国专利cn1 13478490a公开了一种具有预设性能的工业机器人pid控制方法，在传统的位置式pid控制方法的基础上，将位置跟踪误差转换为具有上界的变换误差，通过弱化pid参数对末端执行器稳态跟踪误差的影响，增强对工业机器人控制系统稳态位置跟踪误差需求的约束。但该方法将位置跟踪误差转换为具有上界的变换误差，进而得到控制器的控制输入值，该处理方式弱化了积分项和微分项的作用，虽然有效限制了机器人的绝对定位误差，但是增大了系统的稳态误差，降低了系统的稳定性，当外部负载越大、变换频率越高时系统的稳定性和精度不能得到保障。

3、中国专利cn112518753b公开了一种神经网络迭代补偿的工业机器人轨迹跟踪系统和方法，将笛卡尔空间误差通过bp神经网络映射到机器人各关节误差，再将关节误差输入到神经网络pid模块输出期望关节数据。该方法有效提高了机器人的逆解精度，降低关节误差对运动轨迹精度的影响。该方法通过神经网络将笛卡尔空间误差映射到关节空间上，并直接输入到神经网络pid模块，忽略了实际加工过程中机器人环境噪声的影响，将环境噪声造成的误差也一并输入到控制器中，当环境噪声影响越大时，机器人轨迹精度越低。

4、因此，亟需设计一种面向机器人末端位姿补偿的全闭环反馈控制方法。

技术实现思路

1、针对现有技术的以上缺陷或改进需求，本发明提供了一种面向机器人末端位姿补偿的全闭环反馈控制方法及系统，旨在解决机器人在运行过程中面临的振动、环境噪声干扰问题，有效提高机器人控制的鲁棒性和稳定性。

2、为实现上述目的，按照本发明的一个方面，提供了一种面向机器人末端位姿补偿的全闭环反馈控制方法，包括：s1：根据规划轨迹点位姿和加工速度获取各轨迹段的运行时长，对各轨迹段对应的运行时长进行累积获得各轨迹点位姿对应的时间片；s2：基于机器人的加工时长获得机器人当前时刻对应的时间片，进而获得对应的目标轨迹段，根据目标轨迹段两端点的位姿获得当前时刻的理论位姿；s3：获取当前时刻的实际位姿，根据刚体微分变换理论将所述理论位姿和实际位姿获得每个连杆的微分变换矢量基于运动学误差模型将每个连杆的微分变换矢量转换为对应的关节角误差；s4：以所述关节角误差对非线性pid控制器中的kp、ki和kd参数进行优化得到改进的非线性pid控制器，将所述关节角误差输入所述改进的非线性pid控制器中实现对位姿的补偿。

3、优选的，步骤s2具体为：s21：基于机器人的加工时长获得机器人当前时刻对应的时间片；s22：基于所述时间片和加工速度获得机器人当前时刻所处的轨迹段；s23：对当前时刻所处的轨迹段的两端点处的位姿进行线性插值获得当前时刻的理论位姿。

4、优选的，步骤s3具体为：s31：将所述实际位姿和理论位姿转换为刚体变换矩阵形式和s32：将和输入刚体微分变换方程获得笛卡尔空间的微分平移矢量和微分旋转矢量进而获得连杆j的微分变换矢量；其中，刚体微分变换方程为：

5、

6、其中，i为单位矩阵，分别为绕x轴、y轴、z轴的微分旋转量，分别为沿x轴、y轴、z轴的微分平移量，设

7、s33：基于连杆j的微分变换矢量构建运动学误差模型，仅考虑关节角误差对机器人位姿误差的影响，将机器人位姿误差融入至关节角误差中获得机器人的目标关节角误差。

8、优选的，步骤s33中所述将机器人位姿误差融入至关节角误差中获得机器人的关节角误差具体为：

9、使用机器人的m-dh模型，得到机器人连杆j-1到连杆j的变换公式，基于连杆间的微分变换，得到机器人末端位姿误差与连杆误差的变换关系，当机器人位姿存在误差时，将机器人位姿误差映射至关节空间中获得机器人的雅可比矩阵；

10、基于以下公式获得所述目标关节角误差：

11、

12、其中，qp为目标关节角误差；m为雅可比矩阵，为关节空间中自由度为n的机器人的微分平移矢量，为关节空间中自由度为n的机器人的微分旋转矢量。

13、优选的，步骤s4中优化后的kp、ki和kd参数分别为：

14、kp(ej(t))＝ap+bp(1-g(cpej(t)))

15、ki(ej(t))＝aig(ciej(t))

16、kd(ei(t))＝ad+bd/(1+cdexp(ddei(t)))

17、其中，ej为第j个关节的关节角误差qp(j)，t为控制时间，ap、bp、cp为kp的增益参数，ad、bd、cd、dd为kd的增益参数，ai、ci为ki的增益参数，

18、优选的，改进的非线性pid控制器的控制输入u(t)为：

19、

20、优选的，采用误差绝对值时间积分项和改进的非线性pid控制器的控制输入的平方项构建目标函数，采用遗传算法以所述目标函数最小化为目标求解所述增益参数。

21、优选的，步骤s4中将所述关节角误差输入所述改进的非线性pid控制器之前还包括采用滑动窗口滤波器对所述关节角误差进行滤波得到滤波后的控制关节角误差，所述滑动窗口滤波器的滑动窗口w为：

22、

23、其中，w∈[wmin,wmax]，nmax为刀轴最大转速，fmax为信号最大采样频率，np为机器人的刀轴转速，fp为信号采样频率。

24、优选的，步骤s1根据规划轨迹点位和加工速度获取各轨迹段的运行时长具体为：根据机器人轨迹点位和加工速度信息，基于欧式距离生成各轨迹段的运行时长；步骤s1中轨迹点位姿采用如下方式获得：步骤1：获取待加工工件的表面点云，提取待加工工件的边缘轮廓；步骤2：根据加工需求，确定机器人进给速度和切削深度得到加工位姿信息；步骤3：将加工轨迹的初始姿态和终止姿态转换为四元数，并根据点位距离进行线性插值得到各轨迹点位的姿态信息，得到轨迹点位姿ps。

25、本技术第二方面提供了一种面向机器人末端位姿补偿的全闭环反馈控制系统，包括：第一获取模块：用于根据规划轨迹点位姿和加工速度获取各轨迹段的运行时长，对各轨迹段对应的运行时长进行累积获得各轨迹点位姿对应的时间片；第二获取模块：用于基于机器人的加工时长获得机器人当前时刻对应的时间片，进而获得对应的目标轨迹段，根据目标轨迹段两端点的位姿获得当前时刻的理论位姿；第三获取模块：用于获取当前时刻的实际位姿，根据刚体微分变换理论将所述理论位姿和实际位姿获得每个连杆的微分变换矢量，基于运动学误差模型将每个连杆的微分变换矢量转换为对应的关节角误差；补偿模块：用于以所述关节角误差对非线性pid控制器中的kp、ki和kd参数进行优化得到改进的非线性pid控制器，将所述关节角误差输入所述改进的非线性pid控制器中实现对位姿的补偿。

26、总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的面向机器人末端位姿补偿的全闭环反馈控制方法及系统主要具有以下有益效果：

27、1.相较于一般的机器人闭环反馈控制方法，该方法将笛卡尔空间误差通过雅可比矩阵转换到关节空间中，对每个关节轴进行pid控制；同时考虑到关节误差与pid参数的关系，自定义非线性方程组描述kp、ki和kd与误差之间的对应关系，该方法具有较强的实时性和自适应性，能够更好应对工业机器人加工过程中存在的外部扰动，同时能更好地适应机器人系统此类非线性系统在不同工作区域的变换，实现了对复杂系统复杂环境的自适应pid控制。

28、2.本发明提出一种考虑刀轴转速和采样频率的滤波方法，相对于传统的滑动窗口滤波器，该方法结合实际工况，考虑到机器人实际加工过程中刀轴转速对于机器人系统的振动影响，同时考虑到了机器人数据采样频率对机器人的快速响应和滑动窗口大小的影响，构建了滑动窗口大小与刀轴转速和机器人采样频率之间的关系方程，使得滤波器能更好地适应机器人不同作业条件，能在有效降低噪声的同时保证数据处理的实时性和灵敏度。