一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法与流程

1.本发明涉及机器人技术领域，尤其是一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法。


背景技术：

2.随着机器人技术的迅速发展，越来越多的工业机器人被应用于打磨作业中，替代人工打磨，以降低人工成本和提高生产效率。
3.目前一般不采用对工件模型依赖程度高的离线编程方式，而是采用示教编程方式对机器人进行打磨轨迹的示教，如拖动示教。
4.但是，由于打磨具有工件表面形状不确定以及与工件表面持续接触的固有特性，需要同时向机器人示教打磨位姿轨迹和法向力轨迹，以实现机器人在打磨这种受环境约束任务下的稳定性和柔顺性，而且为满足高精度的要求，机器人在打磨过程中需要控制打磨头的方向与工件表面垂直，并以适当法向力对工件进行打磨，否则容易出现过磨等问题，这导致难以一次性地示教出一条高质量的打磨轨迹，一旦在示教过程中出现失误，比如示教了错误的位姿或者不合适的法向力轨迹，往往需要重新示教整条轨迹，这额外消耗了大量时间，同时增加了生产成本，因此需要示教轨迹的优化和修正。
5.现有技术中，示教轨迹修正最直观的方法是，使用六维力/力矩传感器测量手臂作用力，通过导纳控制算法对轨迹进行修正，但是在打磨过程中，打磨头与工件剧烈接触，在切平面上产生大量的力噪声，难以从六维力/力矩传感器的测量值中分离出手臂作用力。


技术实现要素：

6.针对现有技术的不足，本发明提供一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，本发明通过训练数据采集与处理，神经网络设计与训练，轨迹示教与建模以及轨迹在线优化与修正，实现了一次性示教，缩短了机器人的示教时间，提高了示教的效率和鲁棒性，增强了机器人的柔性打磨作业能力，对操作人员更加安全友好。
7.本发明的技术方案为：一种基于示教轨迹优化与修正的机器人打磨系统，包括机器人、六维力/力矩传感器、打磨头、肌电手环和工控机，所述的机器人为具有位置控制功能的六轴机械臂；
8.所述六维力/力矩传感器安装于所述机器人的末端，用于测量打磨过程中环境或操作人员对所述机器人末端施加的外力，所述打磨头安装于所述六维力/力矩传感器的另一端，与工件直接接触进行打磨，所述肌电手环佩戴于操作人员的右前臂，用于采集手臂的肌电信号，所述工控机与所述机器人和所述六维力/力矩传感器均电连接，与所述的肌电手环通过蓝牙连接，根据六维力/力矩传感器获取到的外力和肌电手环获取的肌电信号向所述机器人发送控制指令，控制机器人带动打磨头在工件表面移动，完成打磨作业。
9.本发明还提供一种基于示教轨迹优化与修正的机器人的打磨方法，包括以下步骤：
10.s1)、训练数据采集与处理，操作人员用手拖动机器人末端分别沿x轴，y轴和z轴进行周期移动，同时，利用工控机读取肌电手环测量得到的手臂八通道原始肌电信号和六维力/力矩传感器测量得到的力信号对这两组信号进行时间戳同步，同时对肌电信号u(k)进行预处理得到扩展肌电信号方阵
11.s2)、构建神经网络模型，并利用步骤s1)中处理的对应时间戳的扩展肌电信号方阵u
*
(k)和三维力信号f
*
(k)组成数据对使用反向传播算法对神经网络进行训练，训练好的神经网络模型表示手臂肌电信号和预测机器人末端所受手臂力的映射关系为
12.式中，γ(u
*
)表示表示神经网络映射函数；
13.s3)、轨迹示教与建模，在拖动示教模式下，对机器人示教在工件表面进行打磨的位姿轨迹βs(t)和垂直于工件表面的法向力轨迹f
zs
(t)，工控机记录示教位姿和法向力轨迹，使用动态运动原语对机器人轨迹从正向和反向进行建模，得到位姿模型和力模型；
14.s4)、在线优化，用力控算法和姿态导纳控制算法在线优化机器人的位姿轨迹，
15.经过姿态导纳控制算法对机器人姿态进行优化，机器人末端会自动地根据环境外力矩调整姿态，从而实现在打磨过程中打磨头方向与工件表面垂直；
16.s5)、在线修正，使用神经网络模型预测手臂对机器人末端作用力，调整位姿和力模型执行速度，通过位置导纳控制，工控机向机器人发送控制指令，在线修正优化后的机器人位置和力轨迹；
17.s6)、循环执行步骤s4)和s5)，直到完成完成示教轨迹的在线优化和修正。
18.作为优选的，步骤s1)中，对所述的肌电信号u(k)进行预处理，具体如下：
19.s11)、对原始肌电信号u(k)进行整流滤波处理，得到肌电信号包络u
env
(k)，计算式为：
[0020][0021]
式中，k表示当前时间戳，m表示预先设计的滤波参数；
[0022]
s12)、对肌电信号包络u
env
(k)进行标准化处理，得到标准化肌电信号u
*
(k)，计算式如下：
[0023][0024]
式中，表示肌电信号包络u
env
(k)的平均值，σ(u
env
)表示肌电信号包络u
env
(k)的标准差；
[0025]
s13)、使用滑动窗口截取标准化肌电信号u
*
(k)，得到扩展肌电信号方阵具体为：
[0026]u*
(k)＝[u
*
(k-w)，u
*
(k-w+1)，
…
，u
*
(k)]；
[0027]
式中，u
*
(k)为标准化肌电信号，w表示滑动窗口长度。
[0028]
作为优选的，步骤s2)中，所述的神经网络模型包括一个输入层，四个卷积模块和一个输出层，所述的神经网络模型以扩展肌电信号方阵u
*
(k)作为输入，以预测三维力信号作为输出。
[0029]
作为优选的，步骤s2)中，每个卷积模块包括一个2d卷积层，批标准化层bn和20％的dropout层，并使用整流线性单元relu进行激活，卷积层的卷积核数量依次设计为4，8，16和32，卷积核大小均设计为2x2，输出层设计为两个单元个数分别为6和3的全连接层，对每个全连接层使用relu进行激活。
[0030]
作为优选的，步骤s2)中，使用反向传播算法对神经网络进行训练，训练得到的最优权重θ
*
满足最小化损失函数l：
[0031][0032]
其中，θ
*
表示训练得到的最优权重，n表示训练集数据个数，f
*
(k)表示三维力信号，表示预测三维力信号。
[0033]
作为优选的，步骤s3)中，所述的位姿轨迹为：
[0034]
βs(t)＝[β
xs
(t)，β
ys
(t)，β
zs
(t)，β
ss
(t)，β
ps
(t)，β
ys
(t)]；
[0035]
其中，β
xs
(t)，β
ys
(t)，β
zs
(t)代表所述机器人末端位置，β
rs
(t)，β
ps
(t)，β
ys
(t)代表所述机器人末端姿态，以rpy旋转角的形式给出。
[0036]
作为优选的，步骤s3)中，使用动态运动原语对机器人末端位姿轨迹从时间轴正向和反向进行建模，得到位姿模型：
[0037][0038][0039][0040][0041]
φi＝exp(-di(s-ci)2)；
[0042]
式中，a和b分别表示位姿模型刚度系数和阻尼系数，g和β0分别表示位姿轨迹的目标值和初始值；τ表示模型时间放缩系数；β1＝[β
1x
，β
1y
，β
1z
，β
1r
，β
1p
，β
1y
]和β2＝[β
2x
，β
2y
，β
2z
，β
2r
，β
2p
，β
2y
]分别表示机器人末端位姿和速度；ωi，di，ci分别表示第i个高斯基函数φi的权重，宽度和中心；m表示选取的高斯基函数数量，s表示模型相位变量，α表示预设的正参数；
[0043]
表示机器人末端加速度、表示机器人末端速度、f(s；ω)表示模型非线性项、表示模型相位变量的一阶导数；
[0044]
作为优选的，步骤s3)中，使用动态运动原语对机器人z轴的法向力轨迹从时间轴正向和反向进行建模，得到力模型：
[0045][0046][0047][0048]
其中τ模型时间放缩系数，c和d分别表示力模型的刚度系数和阻尼系数，fg和f0分别表示力轨迹的目标值和初始值，f1＝f
1z
和f2＝f
2z
分别表示机器人末端z轴力和力的导数，γi表示第i个高斯基函数φi的权重；位姿和力模型由同一个相位变量s驱动，以实现在时间轴上的同步；
[0049]
表示机器人末端z轴力的二阶导数、表示机器人末端z轴力的一阶导数。
[0050]
作为优选的，步骤s4)中，所述的姿态导纳控制算法为：
[0051]
定义r＝h(r，p，y)为从rpy旋转角到对应旋转矩阵的变换，r＝g(r)为从旋转矩阵到对应旋转矢量的变换，h-1
和g-1
为相应的逆变换，则以旋转矢量表示的姿态偏移re为：
[0052][0053]
re(t)＝g(re(t))；
[0054]
其中，rd表示期望姿态，r0表示位姿模型输出姿态对应的旋转矩阵，re表示以旋转矩阵表示的姿态偏移；
[0055]
迭代计算机器人期望姿态β
rd
，β
pd
和β
yd
，以rpy旋转角的形式表示：
[0056][0057][0058]
δre(t+1)＝g-1
(δre(t+1))；
[0059]
rd(t+1)＝δre(t+1)re(t)r0(t+1)；
[0060]
[β
rd
(t+1)，β
pd
(t+1)，β
yd
(t+1)]＝h-1
(rd(t+1))；
[0061]
其中，δ(
·
)表示当前时间步t到下一时间步t+1的增量，dr和kr分别表示姿态导纳控制的阻尼和刚度参数，表示所述机器人末端的角速度，n
*
(t)＝[n
x
(t)，ny(t)，0]为构造机器人末端外力矩，n
x
和ny分别表示六维力/力矩传感器测量得到的x轴和y轴力矩，t表示控制周期；
[0062]
经过姿态导纳控制算法对机器人姿态进行优化，机器人末端会自动地根据环境外力矩调整姿态，从而实现在打磨过程中打磨头方向与工件表面垂直。
[0063]
作为优选的，步骤s4)中，为了满足以适当法向力对工件进行打磨的要求，将力模型的输出作为期望打磨法向力，根据该期望打磨法向力调整期望位置，在位姿模型输出位置β
1z
，速度β
2z
和加速度的基础上，使用力控算法迭代计算机器人的z轴优化期望加速度速度和位置β
zd1
：
[0064][0065][0066][0067]
式中，t表示当前时间步，t表示控制周期，m
z1
和d
z1
分别表示调整力控性能的惯性和阻尼参数，β
zd1
，和的初值分别与位姿模型输出位置β
1z
，速度β
2z
和加速度的初值相同，f
1z
表示期望打磨法向力，同时表示力模型的输出，fz表示六维力/力矩传感器测量得到的z轴打磨法向力。
[0068]
作为优选的，步骤s5)中，肌电手环实时测量得到手臂八通道的原始肌电信号，经过整流滤波和标准化处理后，使用滑动窗口截取肌电信号流，得到扩展肌电信号方阵u
*
(t)作为神经网络模型的输入，根据实时预测手臂在所述机器人末端x轴，y轴和z轴的作用力
[0069]
表示修正轨迹的人类意图，其中，和表示修正位置轨迹的人类意图，表示修正法向力轨迹的人类意图。
[0070]
作为优选的，步骤s5)中，根据手臂对机器人末端作用力和机器人速度的方向关系，改变模型时间放缩系数τ来调整位姿模型和力模型的执行速度；
[0071]
当τ(t)＞0时，运行正向位姿和力模型，即位姿和力模型沿时间轴正向进行迭代；
[0072]
当τ(t)＝0时，模型停止迭代；
[0073]
当τ(t)《0时，运行反向位姿和力模型，即位姿和力模型沿时间轴反向进行迭代，|τ(t)|与模型执行速度呈正相关关系：
[0074][0075]
其中，τ
max
表示模型时间放缩系数的最大值，表示在机器人xy平面上神经网络预测的手臂作用力向量，β
2xy
＝[β
2x
，β
2y
]表示位姿模型输出的速度向量，pm表示预设正参数。
[0076]
作为优选的，步骤s5)中，在机器人的x轴和y轴上使用位置导纳控制算法进行位置轨迹修正，设计算法中的刚度项不为0，使得修正后的位置轨迹可以逐步收敛到示教轨迹；
[0077]
在位姿模型输出位置β
1xy
＝[β
1x
，β
1y
]和速度β
2xy
＝[β
2x
，β
2y
]的基础上，计算沿机器人的x轴和y轴修正后得到的期望速度和位置β
xyd
＝[β
xd
，β
yd
]；
[0078][0079][0080]
其中，d
xy
和k
xy
分别表示位置导纳控制的阻尼和刚度参数，β
xyd
和的初值与位姿模型输出位置β
1xy
和速度β
2xy
相同，t表示控制周期，表示在所述机器人xy平面上的预测手臂作用力向量。
[0081]
作为优选的，步骤s5)中，在机器人的z轴上使用位置导纳控制算法，设置算法刚度项为0，使操作人员可以直接拖动所述机器人末端进行法向力修正，在优化期望位置β
zd1
，速度和加速度的基础上，计算沿机器人的z轴修正后得到的期望加速度速度和位置β
zd
：
[0082][0083][0084][0085]
其中，m
z2
和d
z2
分别表示修正法向力行为的惯性和阻尼参数，位置β
zd
，速度和期望加速度的初值分别与z轴优化期望轨迹位置β
zd1
，速度和加速度的初值相同。
[0086]
作为优选的，步骤s5)中，工控机将更新的期望位姿控制指令βd(t+1)＝[β
xd
(t+1)，β
yd
(t+1)，β
zd
(t+1)，β
rd
(t+1)，β
pd
(t+1)，β
yd
(t+1)]发送给机器人，控制机器人跟随期望位姿，同时保存机器人末端实时位姿β和外力f。
[0087]
本发明的有益效果为：
[0088]
1、本发明在打磨过程中自动地优化示教轨迹中不合理的位姿和法向力，提高了示教的鲁棒性，增强了机器人的柔性打磨作业能力；
[0089]
2、本发明使用神经网络预测手臂对机器人末端的作用力，避免了打磨中切平面力噪声对手臂作用力的测量干扰，通过物理接触方式直观地向机器人传递修正轨迹的意图，降低了修正轨迹的难度；
[0090]
3、本发明通过示教轨迹在线优化和修正实现了一次性示教，避免了重复示教，提高了示教的效率，对操作人员更加安全友好。
附图说明
[0091]
图1是本发明机器人打磨系统的结构示意图；
[0092]
图2是本发明方法的步骤流程图；
[0093]
图3是本发明神经网络模型的结构示意图；
[0094]
图4是本发明在线优化和修正的控制框图；
[0095]
图5是本发明机器人在线修正位置轨迹的示意图；
[0096]
图6是本发明机器人在线修正法向力轨迹的示意图；
[0097]
其中，1-机器人，2-六维力/力矩传感器，3-打磨头，4-肌电手环，5-工控机，6-不需要打磨区域，7-粗糙区域。
具体实施方式
[0098]
下面结合附图对本发明的具体实施方式作进一步说明：
[0099]
实施例1
[0100]
如图1所示，本实施例提供一种基于示教轨迹优化与修正的机器人打磨系统，包括机器人1、六维力/力矩传感器2、打磨头3、肌电手环4和工控机5。机器人1为具有位置控制功能的六轴机械臂，六维力/力矩传感器2安装于机器人1末端，用于测量打磨过程中环境或操作人员对机器人1末端施加的外力，打磨头3安装于六维力/力矩传感器2的另一端，与工件直接接触进行打磨，肌电手环4佩戴于操作人员的右前臂，用于采集手臂的肌电信号，工控机5与所述机器人1和六维力/力矩传感器2均电连接，与肌电手环4通过蓝牙连接，根据获取到的外力和肌电信号向机器人1发送控制指令，控制机器人1带动打磨头3在工件表面移动，完成打磨作业。
[0101]
实施例2
[0102]
如图2所示，本实施例提供一种基于示教轨迹优化与修正的机器人的打磨方法，包括以下步骤：
[0103]
s1)、训练数据采集与处理，操作人员用手拖动机器人1末端分别沿x轴，y轴和z轴进行周期移动，持续10分钟，同时，利用工控机5读取肌电手环4测量得到的手臂八通道原始肌电信号和六维力/力矩传感器测量得到的力信号对这两组信
号进行时间戳同步，同时对肌电信号u(k)进行预处理得到扩展肌电信号方阵
[0104]
对所述的肌电信号u(k)进行预处理，具体如下：
[0105]
s11)、为了滤除原始肌电信号的高频噪声，同时对其幅值进行校正，对原始肌电信号u(k)进行整流滤波处理，得到肌电信号包络u
env
(k)，计算式为：
[0106][0107]
式中，k表示当前时间戳，m表示预先设计的滤波参数；
[0108]
s12)、为了减小训练数据的取值范围差异，使神经网络模型训练效果更好，对肌电信号包络u
env
(k)进行标准化处理，得到标准化肌电信号u
*
(k)，计算式如下：
[0109][0110]
式中，表示肌电信号包络u
env
(k)的平均值，σ(u
env
)表示肌电信号包络u
env
(k)的标准差；
[0111]
s13)、使用滑动窗口截取标准化肌电信号u
*
(k)，得到扩展肌电信号方阵具体为：
[0112]u*
(k)＝[u
*
(k-w)，u
*
(k-w+1)，
…
，u
*
(k)]；
[0113]
式中，u
*
(k)为标准化肌电信号，w表示滑动窗口长度。
[0114]
s2)、构建神经网络模型，并利用步骤s1)中处理的对应时间戳的扩展肌电信号方阵u
*
(k)和三维力信号f
*
(k)组成数据对使用反向传播算法对神经网络进行训练，训练得到的最优权重θ
*
满足最小化损失函数l：
[0115][0116]
其中，θ
*
表示训练得到的最优权重，n表示训练集数据个数，f
*
(k)表示三维力信号，表示预测三维力信号。
[0117]
训练好的神经网络模型表示手臂肌电信号和预测机器人末端所受手臂力的映射关系为其中，γ(u
*
)表示表示神经网络映射函数；
[0118]
如图3所示，本实施例所述的神经网络模型包括一个输入层，四个卷积模块和一个输出层，所述的神经网络模型以扩展肌电信号方阵u
*
(k)作为输入，以预测三维力信号作为输出。每个卷积模块包括一个2d卷积层，批标准化层bn和20％的dropout层，并使用整流线性单元relu进行激活，卷积层的卷积核数量依次设计为4，8，16和32，卷积核大小均设计为2x2，输出层设计为两个单元个数分别为6和3的全连接层，对每个全连接层使用relu进行激活。
[0119]
s3)、轨迹示教与建模，在拖动示教模式下，对机器人示教在工件表面进行打磨的位姿轨迹βs(t)和垂直于工件表面的法向力轨迹f
zs
(t)，工控机记录示教位姿和法向力轨迹，其中，βs(t)＝[β
xs
(t)，β
ys
(t)，β
zs
(t)，β
rs
(t)，β
ps
(t)，β
ys
(t)]；
[0120]
其中，β
xs
(t)，β
ys
(t)，β
zs
(t)代表所述机器人末端位置，β
rs
(t)，β
ps
(t)，β
ys
(t)代表所述机器人末端姿态，以rpy旋转角的形式给出。
[0121]
使用动态运动原语对机器人末端位姿轨迹从时间轴正向和反向进行建模，得到位
姿模型：
[0122][0123][0124][0125][0126]
φi＝exp(-di(s-ci)2)；
[0127]
式中，a和b分别表示位姿模型刚度系数和阻尼系数，g和β0分别表示位姿轨迹的目标值和初始值；τ表示模型时间放缩系数；β1＝[β
1x
，β
1y
，β
1z
，β
1r
，β
1p
，β
1y
]和β2＝[β
2x
，β
2y
，β
2z
，β
2r
，β
2p
，β
2y
]分别表示机器人末端位姿和速度；ωi，di，ci分别表示第i个高斯基函数φi的权重，宽度和中心；m表示选取的高斯基函数数量，s表示模型相位变量，α表示预设的正参数；
[0128]
表示机器人末端加速度、表示机器人末端速度、f(s；ω)表示模型非线性项、表示模型相位变量的一阶导数；
[0129]
使用动态运动原语对机器人z轴的法向力轨迹从时间轴正向和反向进行建模，得到力模型：
[0130][0131][0132][0133]
其中τ模型时间放缩系数，c和d分别表示力模型的刚度系数和阻尼系数，fg和f0分别表示力轨迹的目标值和初始值，f1＝f
1z
和f2＝f
2z
分别表示机器人末端z轴力和力的导数，γi表示第i个高斯基函数φi的权重；位姿和力模型由同一个相位变量s驱动，以实现在时间轴上的同步；表示机器人末端z轴力的二阶导数、表示机器人末端z轴力的一阶导数。
[0134]
s4)、在线优化，用力控算法和姿态导纳控制算法在线优化机器人的位姿轨迹，经过姿态导纳控制算法对机器人姿态进行优化，机器人末端会自动地根据环境外力矩调整姿态，从而实现在打磨过程中打磨头方向与工件表面垂直；
[0135]
如图4所示，为了满足在打磨过程中打磨头3方向与工件表面垂直的要求，所述的姿态导纳控制算法为：
[0136]
定义r＝h(r，p，y)为从rpy旋转角到对应旋转矩阵的变换，r＝g(r)为从旋转矩阵到对应旋转矢量的变换，h-1
和g-1
为相应的逆变换，则以旋转矢量表示的姿态偏移re为：
[0137]
r0(t)＝h(β
1r
(t)，β
1p
(t)，β
1y
(t))；
[0138][0139]
re(t)＝g(re(t))；
[0140]
其中，rd表示期望姿态，r0表示位姿模型输出姿态对应的旋转矩阵，re表示以旋转矩阵表示的姿态偏移；
[0141]
利用在打磨作业中机器人1末端绕z轴的旋转不受限制的特性，构造机器人1末端所受外力矩为n
*
(t)＝[n
x
(t)，ny(t)，0]，其中n
x
和ny分别表示六维力/力矩传感器2测量得到
的x轴和y轴力矩。使用姿态导纳控制算法迭代计算机器人期望姿态β
sd
，β
pd
和β
yd
，以rpy旋转角的形式给出：
[0142][0143][0144]
δre(t+1)＝g-1
(δre(t+1))；
[0145]
rd(t+1)＝δre(t+1)re(t)r0(t+1)；
[0146]
[β
rd
(t+1)，β
pd
(t+1)，β
yd
(t+1)]＝h-1
(rd(t+1))；
[0147]
其中，δ(
·
)表示当前时间步t到下一时间步t+1的增量，dr和kr分别表示姿态导纳控制的阻尼和刚度参数，表示所述机器人1末端的角速度，t表示控制周期；
[0148]
经过姿态导纳控制算法对机器人姿态进行优化，机器人末端会自动地根据环境外力矩调整姿态，从而实现在打磨过程中打磨头方向与工件表面垂直。
[0149]
为了满足以适当法向力对工件进行打磨的要求，将力模型的输出作为期望打磨法向力，根据该期望打磨法向力调整期望位置，在位姿模型输出位置β
1z
，速度β
2z
和加速度的基础上，使用力控算法迭代计算机器人的z轴优化期望加速度速度和位置β
zd1
：
[0150][0151][0152][0153]
式中，t表示当前时间步，t表示控制周期，m
z1
和d
z1
分别表示调整力控性能的惯性和阻尼参数，β
zd1
，和的初值分别与位姿模型输出位置β
1z
，速度β
2z
和加速度的初值相同，f
1z
表示期望打磨法向力，同时表示力模型的输出，fz表示六维力/力矩传感器测量得到的z轴打磨法向力。
[0154]
s5)、在线修正，使用神经网络模型预测手臂对机器人末端作用力，调整位姿和力模型执行速度，通过位置导纳控制，工控机向机器人发送控制指令，在线修正优化后的机器人位置和力轨迹；
[0155]
如图4所示，肌电手环实时测量得到手臂八通道的原始肌电信号，经过整流滤波和标准化处理后，使用滑动窗口截取肌电信号流，得到扩展肌电信号方阵u
*
(t)作为神经网络模型的输入，根据实时预测手臂在所述机器人末端x轴，y轴和z轴的作用力
[0156]
表示修正轨迹的人类意图，其中，和表示修正位置轨迹的人类意图，表示修正法向力轨迹的人类意图。
[0157]
根据手臂对机器人末端作用力和机器人速度的方向关系，改变模型时间放缩系数τ来调整位姿模型和力模型的执行速度；
[0158]
当τ(t)＞0时，运行正向位姿和力模型，即位姿和力模型沿时间轴正向进行迭代；
[0159]
当τ(t)＝0时，模型停止迭代；
[0160]
当τ(t)《0时，运行反向位姿和力模型，即位姿和力模型沿时间轴反向进行迭代，|τ
(t)|与模型执行速度呈正相关关系：
[0161][0162]
其中，τ
max
表示模型时间放缩系数的最大值，表示在机器人xy平面上神经网络预测的手臂作用力向量，β
2xy
＝[β
2x
，β
2y
]表示位姿模型输出的速度向量，pm表示预设正参数。
[0163]
在机器人的x轴和y轴上使用位置导纳控制算法进行位置轨迹修正，设计算法中的刚度项不为0，使得修正后的位置轨迹可以逐步收敛到示教轨迹；
[0164]
在位姿模型输出位置β
1xy
＝[β
1x
，β
1y
]和速度β
2xy
＝[β
2x
，β
2y
]的基础上，计算沿机器人的x轴和y轴修正后得到的期望速度和位置β
xyd
＝[β
xd
，β
yd
]；
[0165][0166][0167]
其中，d
xy
和k
xy
分别表示位置导纳控制的阻尼和刚度参数，β
xyd
和的初值与位姿模型输出位置β
1xy
和速度β
2xy
相同，t表示控制周期，表示在所述机器人xy平面上的预测手臂作用力向量。
[0168]
在机器人的z轴上使用位置导纳控制算法，设置算法刚度项为0，使操作人员可以直接拖动所述机器人末端进行法向力修正，在优化期望位置β
zd1
，速度和加速度的基础上，计算沿机器人的z轴修正后得到的期望加速度速度和位置β
zd
：
[0169][0170][0171][0172]
其中，m
z2
和d
z2
分别表示修正法向力行为的惯性和阻尼参数，位置β
zd
，速度和期望加速度的初值分别与z轴优化期望轨迹位置β
zd1
，速度和加速度的初值相同。
[0173]
工控机将更新的期望位姿控制指令βd(t+1)＝[β
xd
(t+1)，β
yd
(t+1)，β
zd
(t+1)，β
rd
(t+1)，β
pd
(t+1)，β
yd
(t+1)]发送给机器人，控制机器人跟随期望位姿，同时保存机器人末端实时位姿β和外力f。
[0174]
如图5所示，操作人员对位置轨迹进行修正，实线表示示教位置轨迹，虚线表示修正位置轨迹，修正过程可分为四个阶段(5-1)，(5-2)，(5-3)和(5-4)。
[0175]
其中：
[0176]
所述阶段(5-1)为，运行正向模型，机器人1从起点开始沿示教轨迹进行正常打磨作业；
[0177]
所述阶段(5-2)为，当机器人1即将进入不需要打磨区域6时，手臂对机器人1末端施加适当作用力使机器人1末端偏离当前轨迹并减速执行，避开不需要打磨区域6，此时τ(t)＞0且
[0178]
所述阶段(5-3)为，当机器人1末端越过不需要打磨区域6时，松开手臂，此时τ(t)＝τ
max
，机器人1末端快速向示教位置轨迹收敛；
[0179]
所述阶段(5-4)为，机器人1末端收敛回到示教轨迹继续进行正常打磨作业。
[0180]
如图6所示，操作人员对法向力轨迹进行修正，修正过程可分为四个阶段(6-1)，(6-2)，(6-3)和(6-4)。
[0181]
所述阶段(6-1)为，运行正向模型，机器人1从起点开始沿示教轨迹进行正常打磨作业。
[0182]
所述阶段(6-2)为，当机器人1的示教打磨法向力轨迹过小，经过了工件上一处粗糙区域7但无法取得较好的打磨效果时，手臂对机器人1末端施加适当作用力使得τ(t)《0，运行反向模型，机器人1进行轨迹回溯。
[0183]
所述阶段(6-3)为，当机器人1末端回到粗糙区域7时，手臂只对机器人1末端z轴方向施加作用力，增大打磨法向力，使机器人1对粗糙区域7取得较好的打磨效果。
[0184]
所述阶段(6-4)为，当机器人1完成对粗糙区域7的打磨，松开手臂，此时τ(t)＝τ
max
，运行正向模型，机器人1末端沿示教轨迹继续进行正常打磨作业。
[0185]
在修正过程中，在线优化部分使得机器人1末端带动打磨头3始终与工件表面垂直，同时以适当的打磨法向力与工件表面接触，这大大降低了轨迹修正的难度，工控机5实时保存在线优化和修正的最新轨迹，为下一次的打磨作业提供位姿和力的期望轨迹。
[0186]
上述实施例和说明书中描述的只是说明本发明的原理和最佳实施例，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。

技术特征：
1.一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，所述的打磨系统包括机器人(1)、六维力/力矩传感器(2)、打磨头(3)、肌电手环(4)和工控机(5)，所述的机器人(1)为具有位置控制功能的六轴机械臂；所述六维力/力矩传感器(2)安装于所述机器人(1)的末端，用于测量打磨过程中环境或操作人员对所述机器人(1)末端施加的外力，所述打磨头(3)安装于所述六维力/力矩传感器(2)的另一端，与工件直接接触进行打磨，所述肌电手环(4)佩戴于操作人员的右前臂，用于采集手臂的八通道肌电信号，所述工控机(5)与所述机器人(1)和所述六维力/力矩传感器(2)均电连接，与所述的肌电手环(4)通过蓝牙连接，根据六维力/力矩传感器(2)获取到的外力和肌电手环(4)获取的肌电信号向所述机器人(1)发送控制指令，控制机器人(1)带动打磨头(3)在工件表面移动，完成打磨作业；其特征在于，所述的打磨方法包括以下步骤：s1)、训练数据采集与处理，操作人员用手拖动机器人(1)末端分别沿x轴，y轴和z轴进行周期移动，同时，利用工控机(5)读取肌电手环(4)测量得到的手臂八通道原始肌电信号和六维力/力矩传感器(2)测量得到的力信号对这两组信号进行时间戳同步，同时对肌电信号u(k)进行预处理得到扩展肌电信号方阵s2)、模型构建与训练，构建神经网络模型，并利用步骤s1)中处理的对应时间戳的扩展肌电信号方阵u
*
(k)和三维力信号f
*
(k)组成数据对使用反向传播算法对神经网络进行训练，训练得到的最优权重θ
*
满足最小化损失函数l：其中，θ
*
表示训练得到的最优权重，n表示训练集数据个数，f
*
(k)表示三维力信号，表示预测三维力信号；训练好的神经网络模型表示手臂肌电信号和预测机器人(1)末端所受手臂力的映射关系为式中，γ(u
*
)表示表示神经网络映射函数；s3)、轨迹示教与建模，在拖动示教模式下，对机器人(1)示教在工件表面进行打磨的位姿轨迹β
s
(t)和垂直于工件表面的法向力轨迹f
zs
(t)，工控机(5)记录示教位姿和法向力轨迹，使用动态运动原语对机器人(1)轨迹从正向和反向进行建模，得到位姿模型和力模型；其中，所述的位姿轨迹为：β
s
(t)＝[β
xs
(t),β
ys
(t),β
zs
(t),β
rs
(t),β
ps
(t),β
ys
(t)]；其中，β
xs
(t),β
ys
(t),β
zs
(t)代表所述机器人(1)末端位置，β
rs
(t),β
ps
(t),β
ys
(t)代表所述机器人(1)末端姿态，以rpy旋转角的形式给出；s4)、在线优化，用力控算法和姿态导纳控制算法在线优化机器人(1)的位姿轨迹，经过姿态导纳控制算法对机器人(1)姿态进行优化，机器人(1)末端会自动地根据环境外力矩调整姿态，从而实现在打磨过程中打磨头(3)方向与工件表面垂直；s5)、在线修正，使用神经网络模型预测手臂对机器人(1)末端作用力，调整位姿和力模型执行速度，通过位置导纳控制，工控机(5)向机器人(1)发送控制指令，在线修正优化后的机器人(1)位置和力轨迹；
s6)、循环执行步骤s4)和s5)，直到完成示教轨迹的在线优化和修正。2.根据权利要求1所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：步骤s1)中，对所述的肌电信号u(k)进行预处理，具体如下：s11)、对原始肌电信号u(k)进行整流滤波处理，得到肌电信号包络u
env
(k)，计算式为：式中，k表示当前时间戳，m表示预先设计的滤波参数；s12)、对肌电信号包络u
env
(k)进行标准化处理，得到标准化肌电信号u
*
(k)，计算式如下：式中，表示肌电信号包络u
env
(k)的平均值，σ(u
env
)表示肌电信号包络u
env
(k)的标准差；s13)、使用滑动窗口截取标准化肌电信号u
*
(k)，得到扩展肌电信号方阵具体为：u
*
(k)＝[u
*
(k-w),u
*
(k-w+1),
…
,u
*
(k)]；式中，u
*
(k)为标准化肌电信号，w表示滑动窗口长度。3.根据权利要求1所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：步骤s2)中，所述的神经网络模型包括一个输入层，四个卷积模块和一个输出层，所述的神经网络模型以扩展肌电信号方阵u
*
(k)作为输入，以预测三维力信号作为输出；每个卷积模块包括一个2d卷积层，批标准化层bn和20％的dropout层，并使用整流线性单元relu进行激活，卷积层的卷积核数量依次设计为4，8，16和32，卷积核大小均设计为2x2，输出层设计为两个单元个数分别为6和3的全连接层，对每个全连接层使用relu进行激活。4.根据权利要求1所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：步骤s3)中，使用动态运动原语对机器人(1)末端位姿轨迹从时间轴正向和反向进行建模，得到位姿模型：反向进行建模，得到位姿模型：反向进行建模，得到位姿模型：反向进行建模，得到位姿模型：φ
i
＝exp(-d
i
(s-c
i
)2)；式中，a和b分别表示位姿模型刚度系数和阻尼系数，g和β0分别表示位姿轨迹的目标值和初始值；τ表示模型时间放缩系数；β1＝[β
1x
,β
1y
,β
1z
,β
1r
,β
1p
,β
1y
]和β2＝[β
2x
,β
2y
,β
2z
,β
2r
,β
2p
,β
2y
]分别表示机器人(1)末端位姿和速度；ω
i
，d
i
，c
i
分别表示第i个高斯基函数φ
i
的权
重，宽度和中心；m表示选取的高斯基函数数量，s表示模型相位变量，α表示预设的正参数；表示机器人末端加速度、表示机器人末端速度、f(s；ω)表示模型非线性项、表示模型相位变量的一阶导数；使用动态运动原语对机器人(1)z轴的法向力轨迹从时间轴正向和反向进行建模，得到力模型：力模型：力模型：其中τ模型时间放缩系数，c和d分别表示力模型的刚度系数和阻尼系数，f
g
和f0分别表示力轨迹的目标值和初始值，f1＝f
1z
和f2＝f
2z
分别表示机器人(1)末端z轴力和力的导数，γ
i
表示第i个高斯基函数φ
i
的权重；位姿和力模型由同一个相位变量s驱动，以实现在时间轴上的同步；表示机器人末端z轴力的二阶导数、表示机器人末端z轴力的一阶导数。5.根据权利要求1所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：步骤s4)中，所述的姿态导纳控制算法为：定义r＝h(r,p,y)为从rpy旋转角到对应旋转矩阵的变换，r＝g(r)为从旋转矩阵到对应旋转矢量的变换，h-1
和g-1
为相应的逆变换，则以旋转矢量表示的姿态偏移r
e
为：r0(t)＝h(β
1r
(t),β
1p
(t),β
1y
(t))；r
e
(t)＝g(r
e
(t))；其中，r
d
表示期望姿态，r0表示位姿模型输出姿态对应的旋转矩阵，r
e
表示以旋转矩阵表示的姿态偏移；迭代计算机器人(1)期望姿态β
rd
，β
pd
和β
yd
，以rpy旋转角的形式表示：，以rpy旋转角的形式表示：δr
e
(t+1)＝g-1
(δr
e
(t+1))；r
d
(t+1)＝δr
e
(t+1)r
e
(t)r0(t+1)；[β
rd
(t+1),β
pd
(t+1),β
yd
(t+1)]＝h-1
(r
d
(t+1))；其中，δ(
·
)表示当前时间步t到下一时间步t+1的增量，d
r
和k
r
分别表示姿态导纳控制的阻尼和刚度参数，表示所述机器人(1)末端的角速度，n
*
(t)＝[n
x
(t),n
y
(y),0]为构造机器人(1)末端外力矩，n
x
和n
y
分别表示六维力/力矩传感器(2)测量得到的x轴和y轴力矩，t表示控制周期；经过姿态导纳控制算法对机器人(1)姿态进行优化，机器人(1)末端会自动地根据环境外力矩调整姿态，从而实现在打磨过程中打磨头(3)方向与工件表面垂直；为了满足以适当法向力对工件进行打磨的要求，将力模型的输出作为期望打磨法向
力，根据该期望打磨法向力调整期望位置，在位姿模型输出位置β
1z
，速度β
2z
和加速度的基础上，使用力控算法迭代计算机器人(1)的z轴优化期望加速度速度和位置β
zd1
：：：式中，t表示当前时间步，t表示控制周期，m
z1
和d
z1
分别表示调整力控性能的惯性和阻尼参数，β
zd1
，和的初值分别与位姿模型输出位置β
1z
，速度β
2z
和加速度的初值相同，f
1z
表示期望打磨法向力，同时表示力模型的输出，f
z
表示六维力/力矩传感器(2)测量得到的z轴打磨法向力。6.根据权利要求1所述的一种基于示教轨迹优化与修正的机器人(1)打磨系统的打磨方法，其特征在于：步骤s5)中，肌电手环(4)实时测量得到手臂八通道的原始肌电信号，经过整流滤波和标准化处理后，使用滑动窗口截取肌电信号流，得到扩展肌电信号方阵u
*
(t)作为神经网络模型的输入，根据作为神经网络模型的输入，根据实时预测手臂在所述机器人(1)末端x轴，y轴和z轴的作用力和z轴的作用力和z轴的作用力表示修正轨迹的人类意图，其中，和表示修正位置轨迹的人类意图，表示修正法向力轨迹的人类意图。7.根据权利要求6所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：步骤s5)中，根据手臂对机器人(1)末端作用力和机器人(1)速度的方向关系，改变模型时间放缩系数τ来调整位姿模型和力模型的执行速度；当τ(t)>0时，运行正向位姿和力模型，即位姿和力模型沿时间轴正向进行迭代；当τ(t)＝0时，模型停止迭代；当τ(t)<0时，运行反向位姿和力模型，即位姿和力模型沿时间轴反向进行迭代，|τ(t)|与模型执行速度呈正相关关系：其中，τ
max
表示模型时间放缩系数的最大值，表示在机器人(1)xy平面上神经网络预测的手臂作用力向量，β
2xy
＝[β
2x
,β
2y
]表示位姿模型输出的速度向量，p
m
表示预设正参数。8.根据权利要求7所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：步骤s5)中，在机器人(1)的x轴和y轴上使用位置导纳控制算法进行位置轨迹修正，设计算法中的刚度项不为0，使得修正后的位置轨迹可以逐步收敛到示教轨迹；
在位姿模型输出位置β
1xy
＝[β
1x
,β
1y
]和速度β
2xy
＝[β
2x
,β
2y
]的基础上，计算沿机器人(1)的x轴和y轴修正后得到的期望速度和位置β
xyd
＝[β
xd
,β
yd
]；]；其中，d
xy
和k
xy
分别表示位置导纳控制的阻尼和刚度参数，β
xyd
和的初值与位姿模型输出位置β
1xy
和速度β
2xy
相同，t表示控制周期，表示在所述机器人(1)xy平面上的预测手臂作用力向量。9.根据权利要求8所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：步骤s5)中，在机器人(1)的z轴上使用位置导纳控制算法，设置算法刚度项为0，使操作人员可以直接拖动所述机器人(1)末端进行法向力修正，在优化期望位置β
zd1
，速度和加速度的基础上，计算沿机器人(1)的z轴修正后得到的期望加速度速度和位置β
zd
：：：其中，m
z2
和d
z2
分别表示修正法向力行为的惯性和阻尼参数，位置β
zd
，速度和期望加速度的初值分别与z轴优化期望轨迹位置β
zd1
，速度和加速度的初值相同。10.根据权利要求9所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：步骤s5)中，工控机(5)将更新的期望位姿控制指令β
d
(t+1)＝[β
xd
(t+1),β
yd
(t+1),β
zd
(t+1),β
rd
(t+1),β
pd
(t+1),β
yd
(t+1)]发送给机器人(1)，控制机器人(1)跟随期望位姿，同时保存机器人(1)末端实时位姿β和外力f。11.根据权利要求10所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，其特征在于：操作人员对位置轨迹进行修正，包括四个阶段，其中：(5-1)、运行正向模型，机器人(1)从起点开始沿示教轨迹进行正常打磨作业；(5-2)、当机器人(1)即将进入不需要打磨区域(6)时，手臂对机器人(1)末端施加适当作用力使机器人(1)末端偏离当前轨迹并减速执行，避开不需要打磨区域(6)，此时τ(t)>0且(5-3)、当机器人(1)末端越过不需要打磨区域(6)时，松开手臂，此时τ(t)＝τ
max
，机器人(1)末端快速向示教位置轨迹收敛；(5-4)、机器人(1)末端收敛回到示教轨迹继续进行正常打磨作业。12.根据权利要求11所述的一种基于示教轨迹优化与修正的机器人打磨系统的打磨方
法，其特征在于：操作人员对法向力轨迹进行修正，修正过程分为四个阶段，具体为：(6-1)、运行正向模型，机器人(1)从起点开始沿示教轨迹进行正常打磨作业；(6-2)、当机器人(1)的示教打磨法向力轨迹过小，经过了工件上一处粗糙区域(7)但无法取得较好的打磨效果时，手臂对机器人(1)末端施加适当作用力使得τ(t)<0，运行反向模型，机器人(1)进行轨迹回溯；(6-3)、当机器人(1)末端回到粗糙区域(7)时，手臂只对机器人(1)末端z轴方向施加作用力，增大打磨法向力，使机器人(1)对粗糙区域(7)取得较好的打磨效果；(6-4)、当机器人(1)完成对粗糙区域(7)的打磨，松开手臂，此时τ(t)＝τ
max
，运行正向模型，机器人(1)末端沿示教轨迹继续进行正常打磨作业。

技术总结
本发明提供一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法，本发明在打磨之前，根据肌电手环测量的肌电信号和六维力/力矩传感器测量的机器人末端外力，工控机训练神经网络，建立肌电信号到手臂作用力的映射，对示教位姿和力轨迹进行建模，在打磨过程中，根据肌电信号、机器人末端外力和神经网络预测值，工控机向机器人发送控制指令，优化和修正机器人位姿和力轨迹。本发明使用神经网络避免了打磨中切平面力噪声对手臂作用力的测量干扰，实现了在线优化和修正示教轨迹中不合理的位姿和法向力，提高了示教的效率和鲁棒性以及机器人的打磨柔性作业能力。机器人的打磨柔性作业能力。机器人的打磨柔性作业能力。


技术研发人员：叶德禧 黄浩晖 杨辰光
受保护的技术使用者：佛山纽欣肯智能科技有限公司
技术研发日：2022.12.14
技术公布日：2023/8/4