一种机器人腕关节奇异规避方法及系统与流程

1.本发明实施例涉及机器人腕关节控制技术领域，具体涉及一种机器人腕关节奇异规避方法及系统。


背景技术：

2.腕关节奇异点在6r非冗余协作臂常见的问题之一，它可能出现在笛卡尔空间的任何位置，且只与机器人关节5趋于零时的特殊空间位型相关，难以通过限制工作空间的方式进行排除。机械臂经过奇异点附近时，为了保持位置精度，通常需要伴随着笛卡尔空间的降速运动，这往往无法达到空间速度恒定的工艺任务的要求(诸如焊接、点胶等)。针对这一问题，任务排序(task priority sequence)、奇异值分离(singularity isolation)等一系列方法被提出。已有的解决思路大都通过牺牲一定的姿态的精度来对奇异位型进行规避，这种方式对于姿态精度要求不高的应用能解决部分问题。然而，仍然存在一些缺陷，一方面，现有方法往往对过奇异点时姿态误差缺少量化分析，对机器人过奇异点时的最大误差缺少清晰可控的量化指标。


技术实现要素：

3.为此，本发明实施例提供一种机器人腕关节奇异规避方法及系统，以解决现有技术对机器人过奇异点时的最大误差缺少清晰可控的量化指标的技术问题。
4.为了实现上述目的，本发明实施例提供如下技术方案：
5.根据本发明实施例的第一方面，本技术实施例提供了一种机器人腕关节奇异规避方法，所述方法包括：
6.按照预设周期，对机器人在笛卡尔空间中运动时的关节轨迹指令进行抽样检测；
7.利用抽样检测结果，按照当前运动速度预测接下来若干个周期所述机器人的运动姿态；
8.基于预测结果，判断所述机器人前方是否存在奇异位型的奇异点；
9.如果检测到机器人前方存在奇异位型的奇异点，生成角速度偏转姿态
10.利用所述角速度偏转姿态作为修正量和原始目标姿态融合生成新的目标姿态
11.对所述新的目标姿态进行求解，产生新的关节轨迹信号并下发给下位机执行。
12.进一步地，本技术实施例提供的一种机器人腕关节奇异规避方法还包括：
13.如果检测到机器人前方不存在奇异位型的奇异点，对原始目标姿态进行求解，产生关节轨迹信号并下发给下位机执行。
14.进一步地，基于预测结果，判断所述机器人前方是否存在奇异位型的奇异点，包括：
15.基于所述预测结果，获得所述机器人的第五关节空间角度q(t)5以及所述第五关节空间角度q(t)5的一阶差分值
16.判断所述第五关节空间角度q(t)5和所述一阶差分值是否满足第一预设条件，所述第一预设条件为：
17.且或且
18.其中，表示预先设定的第一内圈阈值参数的平均值，表示预先设定的第二内圈阈值参数的平均值；
19.如果满足所述第一预设条件，则判断所述第五关节空间角度q(t)5是否满足第二预设条件，所述第二预设条件为：
[0020][0021]
其中，表示预先设定的外圈阈值参数的平均值；
[0022]
如果满足所述第二预设条件，则所述机器人前方存在奇异位型的奇异点。
[0023]
进一步地，生成角速度偏转姿态包括：
[0024]
利用抽样检测结果计算所述机器人前瞻起始时刻ts工具坐标系t在基坐标中表示的前瞻始位置指令线速度v
t
(ts)和前瞻始位置指令角速度ω
t
(t)；
[0025]
预先设定前瞻时间t
forward
，以所述第一位置矢量p
t
(ts)计算出前瞻始位置s(ts)，利用所述前瞻始位置指令线速度v
t
(ts)，预测前瞻末位置s(te)，所述前瞻末位置s(te)的第一预测公式如下：
[0026]
s(ts)＝0
[0027]
s(te)＝s(ts)+t
forward
||v
t
(ts)||2[0028]
其中，t
forward
表示前瞻时间；
[0029]
利用所述前瞻始位置s(ts)和所述前瞻末位置s(te)，使用二分法搜索所述机器人工具坐标系t在行进路径中最接近奇异位型的奇异点临近位置s(t
final
)和奇异点临近位置姿态，所述奇异点临近位置姿态包括：奇异点临近位置矢量p
t
(s(t
final
))和奇异点临近位置旋转矩阵r
t
(s(t
final
))；
[0030]
利用所述奇异点临近位置矢量p
t
(s(t
final
))和所述奇异点临近位置旋转矩阵r
t
(s(t
final
))，计算所述机器人工具坐标系t的角增速度偏转轴
[0031]
基于所述前瞻始位置指令线速度v
t
(ts)和所述第五关节空间角度q(t)5，计算所述机器人工具坐标系t的偏转角θ(q
final
)，所述偏转角θ(q
final
)的第七计算公式如下：
[0032][0033]
其中，c1为第一预设参数，c2为第二预设参数，c3为第三预设参数；
[0034]
根据所述角增速度偏转轴和所述偏转角θ(q
final
)，利用罗德里格斯公式计算得到角速度偏转姿态所述罗德里格斯公式如下：
[0035][0036]
其中，i为单位矩阵，θ为偏转角θ(q
final
)，为角增速度偏转轴
[0037]
进一步地，利用抽样检测结果计算所述机器人前瞻起始时刻ts工具坐标系t在基坐标中表示的前瞻始位置指令线速度v
t
(ts)和前瞻始位置指令角速度ω
t
(t)，包括：
[0038]
从抽样检测结果得到前瞻起始时刻ts和t
s-1时刻工具坐标系t在基坐标中表示的第一位置矢量p
t
(ts)和第二位置矢量p
t
(t
s-1)以及前瞻起始时刻ts和t
s-1时刻工具坐标系t在基坐标中表示的第一旋转矩阵r
t
(ts)和第二旋转矩阵r
t
(t
s-1)；
[0039]
利用所述第一位置矢量p
t
(ts)和所述第二位置矢量p
t
(t
s-1)计算所述机器人前瞻起始时刻ts工具坐标系t在基坐标中表示的前瞻始位置指令线速度vr(ts)，所述前瞻始位置指令线速度v
t
(ts)的第一计算公式如下：
[0040]vt
(ts)＝(p
t
(ts)-p
t
(t
s-1))/δt
[0041]
其中，δt为抽样预设周期；以及
[0042]
利用所述第一旋转矩阵r
t
(ts)和所述第二旋转矩阵r
t
(t
s-1)计算所述机器人前瞻起始时刻ts工具坐标系t在基坐标中的前瞻始位置指令角速度ω
t
(t)，所述前瞻始位置指令角速度ω
t
(ts)的第二计算公式如下：
[0043][0044]
其中，log表示对矩阵形式的对数变换，t
′
表示转置，v表示对角速度矩阵的向量化，δt为抽样预设周期。
[0045]
进一步地，利用所述前瞻始位置s(ts)和所述前瞻末位置s(te)，使用二分法搜索所述机器人工具坐标系t在行进路径中最接近奇异位型的奇异点临近位置s(t
final
)和奇异点临近位置姿态，包括：
[0046]
利用所述前瞻始位置s(ts)和所述前瞻末位置s(te)，预测中间位置s(th)和中间位置姿态，所述中间位置s(th)的第二预测公式为：
[0047]
s(th)＝0.5(s(ts)+s(te))；
[0048]
以从所述前瞻始位置s(ts)至所述中间位置s(th)作为第一路径，以从所述中间位置s(th)至所述前瞻末位置s(te)作为第二路径，进行二分操作，循环执行所述第二预测公式，预测得到第一路径对应的第一中间位置s(t
h1
)和第二路径对应的第二中间位置s(t
h2
)；其中，第一路径对应的第一前瞻始位置s(t
s1
)和第一前瞻末位置s(t
e1
)分别为所述前瞻始位置s(ts)和所述中间位置s(th)，第二路径对应的第二前瞻始位置s(t
s2
)和第二前瞻末位置s(t
e2
)分别为所述中间位置s(th)和所述前瞻末位置s(te)；
[0049]
对所述第一前瞻始位置s(t
s1
)、所述第一前瞻末位置s(t
e1
)、所述第一中间位置s(t
h1
)对应的位置矢量和旋转矩阵求逆解，得到各自对应的第一关节空间姿态，所述第一关节空间姿态包括：第一始位置关节空间角度q(s(t
s1
))5、第一末位置关节空间角度q(s(t
e1
))5、第一中间位置关节空间角度q(s(t
h1
))5；
[0050]
对所述第二前瞻始位置s(t
s2
)、所述第二前瞻末位置s(t
e2
)、所述第二中间位置s(t
h2
)对应的位置矢量和旋转矩阵求逆解，得到各自对应的第二关节空间姿态，所述第二关节空间姿态包括：第二始位置关节空间角度q(s(t
s2
))5、第二末位置关节空间角度q(s(t
e2
))5、第二中间位置关节空间角度q(s(t
h2
))5；
[0051]
分别判断所述第一关节空间姿态和所述第二关节空间姿态是否满足第三预设条件，所述第三预设条件为：
[0052]
q(s(t
hx
))5＜min(q(s(t
sx
))5，q(s(t
ex
))5)
[0053]
其中，x＝1或2；
[0054]
若所述第一关节空间姿态满足第三预设条件，则执行第三预测公式预测对应的第一中间位置姿态，其中，所述第一中间位置姿态包括：第一中间位置矢量p
t
(s(t
h1
))和第一中间位置旋转矩阵r
t
(s(t
h1
))；并利用所述第一前瞻始位置s(t
s1
)、所述第一前瞻末位置s(t
e1
)、所述第一中间位置s(t
h1
)分别更新所述前瞻始位置s(ts)、所述前瞻末位置s(te)、所述中间位置s(th)，持续进行下一轮的二分操作；
[0055]
若所述第二关节空间姿态满足第三预设条件，则执行第三预测公式预测对应的第二中间位置姿态，其中，所述第二中间位置姿态包括：第二中间位置矢量p
t
(s(t
h2
))和第二中间位置旋转矩阵r
t
(s(t
h2
))；并利用所述第二前瞻始位置s(t
s2
)、所述第二前瞻末位置s(t
e2
)、所述第二中间位置s(t
h2
)分别更新所述前瞻始位置s(ts)、所述前瞻末位置s(te)、所述中间位置s(th)，持续进行下一轮的二分操作；
[0056]
每一轮二分操作执行完，计算更新后的所述前瞻始位置s(ts)和所述前瞻末位置s(te)之间的差值；
[0057]
判断所述差值是否小于预设间值；
[0058]
若所述差值小于预设间值，停止二分操作循环，并将退出时刻更新后的所述中间位置s(th)和对应的中间位置姿态分别记录为奇异点临近位置s(t
final
)和奇异点临近位置姿态，所述奇异点临近位置姿态包括：奇异点临近位置矢量p
t
(s(t
final
))和奇异点临近位置旋转矩阵r
t
(s(t
final
))。
[0059]
进一步地，所述中间位置姿态包括：中间位置矢量p
t
(s(th))和中间位置旋转矩阵r
t
(s(th))，所述中间位置姿态的第三预测公式为：
[0060]
p
t
(s(th))＝p
t
(s(ts))+0.5t
stepvt
(ts)
[0061]
p
t
(s(ts))＝p
t
(ts)
[0062]rt
(s(th))＝exp(0.5t
step
ω
t
(ts))
·rt
(s(ts))
[0063]rt
(s(ts))＝r
t
(ts)
[0064]
t
step
＝[s(th)-s(ts)]/|v
t
(ts)|2[0065]
其中，t
step
为按照前瞻始位置指令线速度v
t
(ts)从前瞻始位置s(ts)运动到中间位置s(th)需要的时间，|v
t
(ts)|2表示前瞻始位置指令线速度v
t
(ts)的二范数。
[0066]
进一步地，利用所述奇异点临近位置矢量p
t
(s(t
nnal
))和所述奇异点临近位置旋转矩阵r
t
(s(t
final
))，计算所述机器人工具坐标系t的角增速度偏转轴包括：
[0067]
利用所述奇异点临近位置矢量p
t
(s(t
final
))和所述奇异点临近位置旋转矩阵r
t
(s(t
final
))，计算所述机器人的奇异点临近位置关节空间角度q
final
，所述奇异点临近位置关节空间角度q
final
的第三计算公式如下：
[0068]qfinal
＝invkin(p
t
(s(t
final
))，r
t
(s(t
final
)))；
[0069]
其中，invkin表示机器人运动学中的逆解公式；
[0070]
基于所述奇异点临近位置关节空间角度q
final
，计算所述机器人工具坐标系t的雅
克比矩阵的第一角速度部分矩阵
[0071]
利用所述奇异点临近位置旋转矩阵r
t
(s(t
final
))，对所述第一角速度部分矩阵进行修正，得到第二角速度部分矩阵所述第二角速度部分矩阵的修正公式如下：
[0072][0073]
从第二角速度部分矩阵中取第4行第1列和第4行第2列的值，得到第一元素j
41
和第一元素j
42
；
[0074]
利用所述第一元素j
41
和第一元素j
42
，计算所述机器人工具坐标系t的角增速度偏转轴所述角增速度偏转轴的第六计算公式如下：
[0075][0076][0077]
其中，[j
42
，-j
41
]
t
表示[j
42
，-j
41
]的转置。
[0078]
进一步地，基于所述奇异点临近位置关节空间角度q
final
，计算所述机器人工具坐标系t的雅克比矩阵的第一角速度部分矩阵包括：
[0079]
基于所述奇异点临近位置关节空间角度q
final
，计算奇异点临近位置轴坐标系相对于基坐标系的第三旋转矩阵其中，m为x、y、z，所述第三旋转矩阵的第四计算公式如下：
[0080][0081]
其中，finkin为机器人运动学中的正运动学公式，为奇异点临近位置轴坐标系相对于基坐标系的位置矢量；
[0082]
根据所述第三旋转矩阵得到各个关节n的z轴单元方向向量在基坐标系中表示的坐标
[0083]
基于所述坐标得到所述机器人工具坐标系t的雅克比矩阵的第一角速度部分矩阵所述第一角速度部分矩阵的第五计算公式如下：
[0084][0085]
根据本发明实施例的第二方面，本技术实施例提供了一种机器人腕关节奇异规避系统，所述系统包括：
[0086]
抽样检测模块，用于按照预设周期，对机器人在笛卡尔空间中运动时的关节轨迹指令进行抽样检测；
[0087]
奇异点前瞻检测模块，用于利用抽样检测结果，按照当前运动速度预测接下来若干个周期所述机器人的运动姿态；基于预测结果，判断所述机器人前方是否存在奇异位型的奇异点；
[0088]
姿态修正模块，用于如果检测到机器人前方存在奇异位型的奇异点，生成角速度偏转姿态
[0089]
姿态融合模块，用于利用所述角速度偏转姿态作为修正量和原始目标姿态融合生成新的目标姿态
[0090]
逆解模块，用于对所述新的目标姿态进行求解，产生新的关节轨迹信号并下发给下位机执行。
[0091]
与现有技术相比，本技术实施例提供的一种机器人腕关节奇异规避方法及系统，可应用于要求工作空间较大，且要求轨迹严格服从位置和时序要求的应用场景，如大型车辆焊接、建筑物外围点胶等。本发明实施例能够精确执行位置和速度指令，且控制姿态偏差精度在设定的范围内，因此可以让机器人无需减速，且姿态稳定地通过奇异位型的奇异点，从而保证工艺高品质的完成。对于已经搭载了逆解模块的商用机器人，本发明实施例可以直接对原来有问题的离散轨迹点逐一进行修正，修改后的轨迹和原轨迹一一对应，这种修正只会发生在奇异点附近，对奇异点附近之外的其他轨迹没有影响。因此，本发明实施例可以作为一个附加模块在所有同构性的商用机器人上，具有较好的通用性。
附图说明
[0092]
为了更清楚地说明本发明的实施方式或现有技术中的技术方案，下面将对实施方式或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是示例性的，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图引伸获得其它的实施附图。
[0093]
本说明书所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。
[0094]
图1为本发明实施例提供的一种机器人腕关节奇异规避系统的结构示意图；
[0095]
图2为本发明实施例提供的一种机器人腕关节奇异规避方法的流程示意图；
[0096]
图3为本发明实施例提供的一种机器人腕关节奇异规避方法中生成角速度偏转姿
态的流程示意图。
具体实施方式
[0097]
以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0098]
本发明实施例的思路为：当机器人在空间中运动时，每隔一定时间对机器人笛卡尔空间的指令轨迹信号进行抽样。抽样之后，通过一个前瞻算法计算机器人按照当前运动速度运动的情况下接下来几个周期的运动姿态。如果检测到机器人奇异姿态在前方存在，将根据一定的规则对当前的指令的产生一个修正量，修正后的信息和原始目标姿态融合生成新的目标姿态。最后，融合后的信息将下发给原有控制器进行求解，产生新的关节轨迹下发给下位机执行。
[0099]
如图1所示，本技术实施例提供了一种机器人腕关节奇异规避系统包括：抽样检测模块1、奇异点前瞻检测模块2、姿态修正模块3、姿态融合模块4、逆解模块5。
[0100]
具体地，抽样检测模块1用于按照预设周期，对机器人在笛卡尔空间中运动时的关节轨迹指令进行抽样检测；奇异点前瞻检测模块2用于利用抽样检测结果，按照当前运动速度预测接下来若干个周期所述机器人的运动姿态；基于预测结果，判断所述机器人前方是否存在奇异位型的奇异点；姿态修正模块3用于如果检测到机器人前方存在奇异位型的奇异点，生成角速度偏转姿态姿态融合模块4用于利用所述角速度偏转姿态作为修正量和原始目标姿态融合生成新的目标姿态逆解模块5用于对所述新的目标姿态进行求解，产生新的关节轨迹信号并下发给下位机执行。如果检测到机器人前方不存在奇异位型的奇异点，逆解模块5还用于对原始目标姿态进行求解，产生关节轨迹信号并下发给下位机执行。
[0101]
本技术实施例提供的一种机器人腕关节奇异规避系统，通过在奇异点附近的姿态的在线修正，使得机器人能通过牺牲一定的位姿规避奇异点。与现有技术相比，本发明实施例使偏转误差和偏转范围达到可调性最大的优点，同时，产生的轨迹在速度层面一阶连续，可直接用于关节控制执行。
[0102]
与上述公开的一种机器人腕关节奇异规避系统相对应，本发明实施例还公开了一种机器人腕关节奇异规避方法。以下结合上述描述的一种机器人腕关节奇异规避系统详细介绍本发明实施例中公开的一种机器人腕关节奇异规避方法。
[0103]
如图2所示，以下对本技术实施例提供的一种机器人腕关节奇异规避方法具体步骤进行详细描述。
[0104]
步骤s11：通过抽样检测模块1按照预设周期，对机器人在笛卡尔空间中运动时的关节轨迹指令进行抽样检测。
[0105]
本发明实施例中，抽样检测算法是一个独立于主控制器线程的一个独立的线程。
独立抽样检测频率与主控制器的抽样频率相同。
[0106]
步骤s12：通过奇异点前瞻检测模块2利用抽样检测结果，按照当前运动速度预测接下来若干个周期所述机器人的运动姿态。
[0107]
步骤s13：通过奇异点前瞻检测模块2基于预测结果，判断所述机器人前方是否存在奇异位型的奇异点。
[0108]
上述步骤12和步骤13即为前瞻检测，其主要目的是寻找机器人运动方向前方是否会趋向腕关节奇异点，并找到机器人以当前速度前进前方是否进入腕关节奇异区域范围。在奇异点前瞻检测模块2中判断机器人进入腕关节奇异区域的条件有两个。
[0109]
具体地，在本发明实施例中，基于预测结果，判断所述机器人前方是否存在奇异位型的奇异点，具体包括以下步骤：基于所述预测结果，获得所述机器人的第五关节空间角度q(t)5以及所述第五关节空间角度q(t)5的一阶差分值判断所述第五关节空间角度q(t)5和所述一阶差分值是否满足第一预设条件，所述第一预设条件为：
[0110]
且或且
[0111]
其中，表示预先设定的第一内圈阈值参数的平均值，表示预先设定的第二内圈阈值参数的平均值；如果满足所述第一预设条件，则判断所述第五关节空间角度q(t)5是否满足第二预设条件，所述第二预设条件为：
[0112][0113]
其中，表示预先设定的外圈阈值参数的平均值；如果满足上述第二预设条件，则所述机器人前方存在奇异位型的奇异点。如果不满足上述第一预设条件或上述第二预设条件，则所述机器人前方不存在奇异位型的奇异点。
[0114]
步骤s14：如果检测到机器人前方不存在奇异位型的奇异点，则通过逆解模块5对原始目标姿态进行求解，产生关节轨迹信号并下发给下位机执行。
[0115]
步骤s15：如果检测到机器人前方存在奇异位型的奇异点，则通过姿态修正模块3生成角速度偏转姿态
[0116]
进一步地，以下对本发明实施例中公开的生成角速度偏转姿态的具体步骤进行描述。
[0117]
步骤s21：利用抽样检测结果计算所述机器人前瞻起始时刻ts工具坐标系t在基坐标中表示的前瞻始位置指令线速度v
t
(ts)和前瞻始位置指令角速度ω
t
(t)。
[0118]
更进一步地，上述步骤s21具体包括：从抽样检测结果得到前瞻起始时刻ts和t
s-1时刻工具坐标系t在基坐标中表示的第一位置矢量p
t
(ts)和第二位置矢量p
t
(t
s-1)以及前瞻起始时刻ts和t
s-1时刻工具坐标系t在基坐标中表示的第一旋转矩阵r
t
(ts)和第二旋转矩阵r
t
(t
s-1)；利用所述第一位置矢量p
t
(ts)和所述第二位置矢量p
t
(t
s-1)计算所述机器人前瞻起始时刻ts工具坐标系t在基坐标中表示的前瞻始位置指令线速度v
t
(ts)，所述前瞻始位置指令线速度v
t
(ts)的第一计算公式如下：
[0119]vt
(ts)＝(p
t
(ts)-p
t
(t
s-1))/δt
[0120]
其中，δt为抽样预设周期；以及利用所述第一旋转矩阵r
t
(ts)和所述第二旋转矩阵r
t
(t
s-1)计算所述机器人前瞻起始时刻ts工具坐标系t在基坐标中的前瞻始位置指令角速度ω
t
(t)，所述前瞻始位置指令角速度ω
t
(ts)的第二计算公式如下：
[0121][0122]
其中，log表示对矩阵形式的对数变换，t
′
表示转置，v表示对角速度矩阵的向量化，δt为抽样预设周期。
[0123]
步骤s22：预先设定前瞻时间t
forward
，以所述第一位置矢量p
t
(ts)计算出前瞻始位置s(ts)，利用所述前瞻始位置指令线速度v
t
(ts)，预测前瞻末位置s(te)，所述前瞻末位置s(te)的第一预测公式如下：
[0124]
s(ts)＝0
[0125]
s(te)＝s(ts)+t
forward
||v
t
(ts)||2[0126]
其中，t
forward
表示前瞻时间；
[0127]
步骤s23：利用所述前瞻始位置s(ts)和所述前瞻末位置s(te)，使用二分法搜索所述机器人工具坐标系t在行进路径中最接近奇异位型的奇异点临近位置s(t
final
)和奇异点临近位置姿态，所述奇异点临近位置姿态包括：奇异点临近位置矢量p
t
(s(t
final
))和奇异点临近位置旋转矩阵r
t
(s(t
final
))。
[0128]
进一步地，得到上述奇异点临近位置s(t
final
)和奇异点临近位置姿态，具体包括以下步骤：利用所述前瞻始位置s(ts)和所述前瞻末位置s(te)，预测中间位置s(th)和中间位置姿态，所述中间位置s(th)的第二预测公式为：
[0129]
s(th)＝0.5(s(ts)+s(te))；
[0130]
其中，所述中间位置姿态包括：中间位置矢量p
t
(s(th))和中间位置旋转矩阵r
t
(s(th))，所述中间位置姿态的第三预测公式为：
[0131]
p
t
(s(th))＝p
t
(s(ts))+0.5t
stepvt
(ts)
[0132]
p
t
(s(ts))＝p
t
(ts)
[0133]rt
(s(th))＝exp(0.5t
step
ω
t
(ts))
·rt
(s(ts))
[0134]rt
(s(ts))＝r
t
(ts)
[0135]
t
step
＝[s(th)-s(ts)]/|v
t
(ts)|2[0136]
其中，t
step
为按照前瞻始位置指令线速度v
t
(ts)从前瞻始位置s(ts)运动到中间位置s(th)需要的时间，|v
t
(ts)|2表示前瞻始位置指令线速度v
t
(ts)的二范数；以从所述前瞻始位置s(ts)至所述中间位置s(th)作为第一路径，以从所述中间位置s(th)至所述前瞻末位置s(te)作为第二路径，进行二分操作，循环执行所述第二预测公式，预测得到第一路径对应的第一中间位置s(t
h1
)和第二路径对应的第二中间位置s(t
h2
)；其中，第一路径对应的第一前瞻始位置s(t
s1
)和第一前瞻末位置s(t
e1
)分别为所述前瞻始位置s(ts)和所述中间位置s(th)，第二路径对应的第二前瞻始位置s(t
s2
)和第二前瞻末位置s(t
e2
)分别为所述中间位置s(th)和所述前瞻末位置s(te)；对所述第一前瞻始位置s(t
s1
)、所述第一前瞻末位置s(t
e1
)、所述第一中间位置s(t
h1
)对应的位置矢量和旋转矩阵求逆解，得到各自对应的第一关节空间姿态，所述第一关节空间姿态包括：第一始位置关节空间角度q(s(t
s1
))5、第一末位置关节空间角度q(s(t
e1
))5、第一中间位置关节空间角度q(s(t
h1
))5；对所述第二前瞻始位置s(t
s2
)、所述第二前瞻末位置s(t
e2
)、所述第二中间位置s(t
h2
)对应的位置矢量和旋转
矩阵求逆解，得到各自对应的第二关节空间姿态，所述第二关节空间姿态包括：第二始位置关节空间角度q(s(t
s2
))5、第二末位置关节空间角度q(s(t
e2
))5、第二中间位置关节空间角度q(s(t
h2
))5；分别判断所述第一关节空间姿态和所述第二关节空间姿态是否满足第三预设条件，所述第三预设条件为：
[0137]
q(s(t
hx
))5＜min(q(s(t
sx
))5，q(s(t
ex
))5)
[0138]
其中，x＝1或2；若所述第一关节空间姿态满足第三预设条件，则执行第三预测公式预测对应的第一中间位置姿态，其中，所述第一中间位置姿态包括：第一中间位置矢量p
t
(s(t
h1
))和第一中间位置旋转矩阵r
t
(s(t
h1
))；并利用所述第一前瞻始位置s(t
s1
)、所述第一前瞻末位置s(t
e1
)、所述第一中间位置s(t
h1
)分别更新所述前瞻始位置s(ts)、所述前瞻末位置s(te)、所述中间位置s(th)，持续进行下一轮的二分操作；若所述第二关节空间姿态满足第三预设条件，则执行第三预测公式预测对应的第二中间位置姿态，其中，所述第二中间位置姿态包括：第二中间位置矢量p
t
(s(t
h2
))和第二中间位置旋转矩阵r
t
(s(t
h2
))；并利用所述第二前瞻始位置s(t
s2
)、所述第二前瞻末位置s(t
e2
)、所述第二中间位置s(t
h2
)分别更新所述前瞻始位置s(ts)、所述前瞻末位置s(te)、所述中间位置s(th)，持续进行下一轮的二分操作；每一轮二分操作执行完，计算更新后的所述前瞻始位置s(ts)和所述前瞻末位置s(te)之间的差值；判断所述差值是否小于预设间值；若所述差值小于预设间值，停止二分操作循环，并将退出时刻更新后的所述中间位置s(th)和对应的中间位置姿态分别记录为奇异点临近位置s(t
final
)和奇异点临近位置姿态，所述奇异点临近位置姿态包括：奇异点临近位置矢量p
t
(s(t
dinal
))和奇异点临近位置旋转矩阵r
t
(s(t
final
))。
[0139]
步骤s24：利用所述奇异点临近位置矢量p
t
(s(t
final
))和所述奇异点临近位置旋转矩阵r
t
(s(t
final
))，计算所述机器人工具坐标系t的角增速度偏转轴
[0140]
进一步地，上述步骤s24具体包括以下步骤：利用所述奇异点临近位置矢量p
t
(s(t
final
))和所述奇异点临近位置旋转矩阵r
t
(s(t
final
))，计算所述机器人的奇异点临近位置关节空间角度q
final
，所述奇异点临近位置关节空间角度q
final
的第三计算公式如下：
[0141]qfinal
＝invkin(p
t
(s(t
final
))，r
t
(s(t
final
)))；
[0142]
其中，invkin表示机器人运动学中的逆解公式；
[0143]
基于所述奇异点临近位置关节空间角度q
final
，计算所述机器人工具坐标系t的雅克比矩阵的第一角速度部分矩阵其具体包括：基于所述奇异点临近位置关节空间角度q
final
，计算奇异点临近位置轴坐标系相对于基坐标系的第三旋转矩阵其中，m为x、y、z，所述第三旋转矩阵的第四计算公式如下：
[0144][0145]
其中，finkin为机器人运动学中的正运动学公式，为奇异点临近位置轴坐标系相对于基坐标系的位置矢量；
[0146]
根据所述第三旋转矩阵得到各个关节n的z轴单元方向向量在基坐标系中表示的坐标基于所述坐标得到所述机器人工具坐标系t的雅克比矩阵的第一角
速度部分矩阵所述第一角速度部分矩阵的第五计算公式如下：
[0147][0148]
之后再利用所述奇异点临近位置旋转矩阵r
t
(s(t
final
))，对所述第一角速度部分矩阵进行修正，得到第二角速度部分矩阵所述第二角速度部分矩阵的修正公式如下：
[0149][0150]
从第二角速度部分矩阵中取第4行第1列和第4行第2列的值，得到第一元素j
41
和第一元素j
42
；利用所述第一元素j
41
和第一元素j
42
，计算所述机器人工具坐标系t的角增速度偏转轴所述角增速度偏转轴的第六计算公式如下：
[0151][0152][0153]
其中，[j
42
，-j
41
]
t
表示[j
42
，-j
41
]的转置。
[0154]
步骤s25：基于所述前瞻始位置指令线速度v
t
(ts)和所述第五关节空间角度q(t)5，计算所述机器人工具坐标系t的偏转角θ(q
final
)，所述偏转角θ(q
final
)的第七计算公式如下：
[0155][0156]
其中，c1为第一预设参数，c2为第二预设参数，c3为第三预设参数。
[0157]
偏转角θ(q
final
)的求解是使用上述第七计算公式，可见，偏转角θ(q
final
)与前瞻始位置指令线速度v
t
(ts)成正比，偏转角θ(q
final
)与机器人当前第五关节空间角度q(t)5成反比。可以通过调整c1、c2和c3的值，调整机器人在奇异点附近的行为。
[0158]
步骤s26：根据所述角增速度偏转轴和所述偏转角θ(q
final
)，利用罗德里格斯公式计算得到角速度偏转姿态所述罗德里格斯公式如下：
[0159][0160]
其中，i为单位矩阵，θ为偏转角θ(q
final
)，为角增速度偏转轴
[0161]
在本发明实施例中，姿态修正模块3是一种在线的轨迹修正器，该模块的输入只依赖于当前指令，不依赖于提前规划的全局信息，因此本发明实施例也可应用于笛卡尔空间的人工示教操作。
[0162]
步骤s16：通过姿态融合模块4利用所述角速度偏转姿态作为修正量和原始目标姿态融合生成新的目标姿态生成新的目标姿态的融合公式如下：
[0163][0164]
步骤s17：通过逆解模块5对所述新的目标姿态进行求解，产生新的关节轨迹信号并下发给下位机执行。
[0165]
与现有技术相比，本技术实施例提供的一种机器人腕关节奇异规避方法及系统，可应用于要求工作空间较大，且要求轨迹严格服从位置和时序要求的应用场景，如大型车辆焊接、建筑物外围点胶等。本发明实施例能够精确执行位置和速度指令，且控制姿态偏差精度在设定的范围内，因此可以让机器人无需减速，且姿态稳定地通过奇异位型的奇异点，从而保证工艺高品质的完成。对于已经搭载了逆解模块的商用机器人，本发明实施例可以直接对原来有问题的离散轨迹点逐一进行修正，修改后的轨迹和原轨迹一一对应，这种修正只会发生在奇异点附近，对奇异点附近之外的其他轨迹没有影响。因此，本发明实施例可以作为一个附加模块在所有同构性的商用机器人上，具有较好的通用性。
[0166]
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。