一种机械臂奇异点的通用规避方法与系统与流程

1.本发明一种机械臂奇异点的通用规避方法与系统，属于机械臂奇异点的通用规避方法与 系统技术领域。


背景技术：

2.工业机器人、协作机器人、医疗辅助机械臂等机械臂领域的研究和使用也在不断推进。 尤其是在工业领域，工业机器人的出现和应用颠覆了传统工业模式，节省了大量人力物力和 劳动生产成本，大大提高了生产效率，替代人员进行危险工作环境中的生产工作，避免了大 量人身事故的发生。，具有重大的意义，对于推动工业生产的自动化程度具有重大作用。
3.同时，由于机械臂的结构特性，不可避免的会遇到奇异点问题。机械臂的奇异点问题 是，机械臂在笛卡尔空间中以有限速度运动某些位形处，机械臂会失去一个或多个自由度， 机械臂若要继续运动，则所需关节速度很大甚至无穷大，在雅可比矩阵上的表现是矩阵的行 列式值为零或接近于零，雅可比矩阵不可逆，使用雅可比矩阵的逆求解关节速度失效或求解 出较大的关节速度，导致机械臂的轨迹跟踪性能以及整个系统的稳定可靠性大大降低。奇异 点问题在机械臂的笛卡尔空间运动规划中无法避免，因而需要一种通用的解决办法来降低其 影响。
4.球型腕关节机器人是末端连续三轴交于一点的机械臂，其末端连续三轴交于一点的特性 也致使在使用雅可比矩阵进行奇异性分析时较为简单；而非球型腕关节机械臂的末端三轴则 没有交会于一点，其末端位置与姿态高度耦合，在使用雅可比矩阵进行奇异性分析和规避较 为困难，计算成本较大。
5.申请号为202011439303.5，名称为一种非球形手腕机械臂奇异点规避方法和系统的中 国发明专利公开了利用雅克比矩阵变化方法实现了非球形手腕机械臂的奇异因子推导，但是 对于部分非球形腕关节机械臂而言，由于其末端位置和姿态的高度耦合以及构型的特殊性， 其雅可比矩阵相当复杂，若只使用雅可比矩阵求解其行列式的数值解在计算成本上尚可接 受，但对雅可比矩阵进行矩阵变换求解子矩阵、对子矩阵求逆、求解奇异因子等一系列变换 求解是十分困难的。
6.因此，为了解决不同机械臂对于奇异性分析和规避的难度不同问题，本发明提出了一种 适用于大多数机械臂的奇异点通用规避方法和系统。


技术实现要素：

7.本发明为了克服现有技术中存在的不足，所要解决的技术问题为：提供一种机械臂奇异 点的通用规避方法的改进。
8.为了解决上述技术问题，本发明采用的技术方案为：一种机械臂奇异点的通用规避方 法，其包括如下步骤：
9.步骤一：首先建立机械臂的运动学模型，建立参考关节坐标系，利用旋量理论计算
各关 节旋量，并求解雅可比矩阵；
10.步骤二：笛卡尔轨迹经运动学变换为关节轨迹，再经插值，使用轨迹点的关节角度值及 雅可比矩阵求得条件数；
11.步骤三：使用条件数判断机械臂是否接近奇异点，在判定即将接近奇异点后，机械臂使 用避奇异算法，计算避奇异轨迹点，直至生成的轨迹点的雅可比矩阵条件数满足远离奇异点 状态且轨迹点重新回到原本笛卡尔轨迹上；
12.步骤四：规划避奇异轨迹后，对规划的避奇异轨迹进行平滑处理以及轨迹插值，得到最 终的输出避奇异轨迹。
13.所述步骤一具体过程为：
14.首先以机械臂某关节轴建立参考坐标系，根据参考坐标系使用旋量理论计算各关节轴方 向向量和位置向量
15.使用旋量法计算机械臂雅可比矩阵，将各关节轴方向向量和位置向量带入雅可比矩 阵，雅可比矩阵j(θ)的计算公式如下：
16.j(θ)＝[$
1 $
2 $3ꢀ…ꢀ
$
n
]；
[0017]
上式中：θ为轨迹点的关节角度值。
[0018]
所述步骤二中条件数的计算公式为：
[0019][0020]
所述步骤三中判断机械臂是否接近奇异点的判断条件为：
[0021]
在某轨迹点处，若对应的条件数为减小趋势，且处于奇异影响阈值c
n
和安全阈值c_sec之 间，则判断机械臂运动正接近奇异点。
[0022]
所述步骤三中采用避奇异算法规划避奇异轨迹的步骤为：
[0023]
避奇异算法采用速度势场算法计算排斥速度与吸引速度，当条件数c为减小趋势时，即 接近奇异点过程，将判断为奇异点的轨迹点记录为避奇异轨迹的起始点；
[0024]
以奇异点构成的一排斥势场，将排斥势场映射到关节空间中，求得关节空间中关节j的 排斥速度v
rep
为：
[0025]
上式中：k为斥力系数，c_sec为安全阈值，c0为奇异影响阈值，c
θj
′
为条件数c对于关节 角度值θ
j
的偏导数；
[0026][0027]
上式中： (θ1，θ2，
…
，θ
n
)为该轨迹关节角度值；
[0028]
将求得的关节排斥速度使用雅可比矩阵推导至笛卡尔空间，求得由奇异点引起的笛卡尔 空间排斥速度v
c_rep
；
[0029]
吸引速度为笛卡尔空间速度，分为两部分，驱动速度v
dri
和修正速度v
adj
，驱动速度v
dri
与原本笛卡尔空间运动速度v
c
同向，有v
dri
＝v
c
‑
v
c_rep
l
c
，其中l
c
为原本笛卡尔轨迹方向向 量；
[0030]
修正速度v
adj
垂直于原本笛卡尔空间轨迹，有v
adj
＝μδx，其中μ为修正速度系数，
δx为 垂直于笛卡尔轨迹的偏移量；
[0031]
再使用雅克比逆矩阵将v
dri
、v
adj
推导至关节空间，与v
rep
(θ
j
)求和，求得最后关节速度 组；
[0032]
使用该组关节速度运行一个时间步长，运动到下一个轨迹点，继续使用速度势场法求取 关节速度组和以该组关节速度运动到的轨迹点，直至新生成的轨迹点达到满足脱离奇异条 件，其中脱离奇异条件为条件数在安全阈值上且生成的轨迹点重新回到原本笛卡尔轨迹上。
[0033]
一种机械臂奇异点的通用规避系统，包括客户端控制显示模块、通信模块、运动控制模 块、电机驱动模块、外围设备和奇异点规避规划模块，其中通信模块，用于客户端控制显示 模块与运动控制模块之间的通信，客户端控制显示模块与外围设备之间的通信，外围设备与 运动控制模块之间的通信，运动控制模块与电机驱动模块之间的通信；
[0034]
所述客户端控制显示模块用于用户编辑控制指令、设置机械臂运动状态，并将设定的指 令、目标位置机械臂状态发送到运动控制模块，同时显示机械臂的参数、状态；
[0035]
所述运动控制模块用于接收客户端控制显示模块发送的指令、机械臂运动解析、机械臂 正逆运动学求解、笛卡尔空间运动规划、轨迹的插补、外围设备的控制；
[0036]
所述奇异点规避规划模块用于奇异点规避轨迹的规划计算、所得轨迹的平滑处理；
[0037]
所述电机驱动模块用于将运动控制模块发送的每个插值周期内的位移转换成脉冲个数， 并将脉冲个数转换成波形信号反馈给驱动电机，以及电机数据的反馈过程。
[0038]
所述运动控制模块包括运动解析模块、机械臂正逆运动学求解模块、笛卡尔空间运动规 划模块、轨迹插值模块，其中运动解析模块用于将用户编写的运动控制指令解析为机械臂控 制器内部指令；
[0039]
机械臂正逆运动学求解模块，用于实现机械臂关节角坐标系和机械臂笛卡尔空间坐标系 之间的坐标变换运算；
[0040]
笛卡尔空间运动规划模块，用于机械臂在笛卡尔空间规划末端执行器的运动轨迹，并将 笛卡尔空间规划的轨迹点发送到机械臂正逆运动学求解模块，转换成关节空间中关节角度 值；
[0041]
轨迹插值模块，用于将笛卡尔空间轨迹或在奇异点规避规划模块得到的运动轨迹，输出 到轨迹插值模块中，轨迹插值模块将整个运动轨迹在关节空间中按照插值周期进行插补，将 该段插值周期内的各轴位移值发送给电机驱动模块。
[0042]
所述奇异点规避规划模块包括奇异点判断检测模块、奇异点规避轨迹计算模块、轨迹平 滑模块，其中奇异点判断检测模块用于在轨迹插值过程中，实时计算关节轨迹点是否满足接 近奇异点条件数；
[0043]
奇异点规避轨迹计算模块通过在机械臂运动中接近奇异点时，使用以速度势场算法为主 的规划算法，规划避奇异轨迹点；
[0044]
轨迹平滑模块，用于平滑规划得到的避奇异轨迹，使用五次b样条曲线平滑轨迹并输 出到运动控制模块。
[0045]
本发明相对于现有技术具备的有益效果为：本发明提供的机械臂奇异点的通用规避方法 只采用机械臂雅可比矩阵的数值解，省去了对于雅可比矩阵解析解的变换求解等
计算成本大 的操作。采用了使用梯度判断奇异点方位，同时设置排斥速度避开奇异点，保证了雅可比矩 阵的逆的存在，以一种计算成本较小的方法进行避奇异操作。
附图说明
[0046]
下面结合附图对本发明做进一步说明：
[0047]
图1为本发明通用规避方法的流程图；
[0048]
图2为本发明通用规避系统的电路结构示意图。
具体实施方式
[0049]
如图1和图2所示，本发明提供了一种具有较高可靠性的，无需提前计算奇异点具体位 型，简化了奇异点分析过程，适用于大多数机械臂的通用机械臂奇异点规避方法与系统。
[0050]
本发明通用机械臂奇异点规避方法采用了如下的技术方案：
[0051]
首先建立机械臂的运动学模型，建立参考关节坐标系，利用旋量理论计算各关节旋量， 并求解雅可比矩阵；
[0052]
根据雅可比矩阵计算雅可比矩阵条件数，使用条件数判断是否接近奇异点，当条件数减 小且接近安全阈值，认为机械臂即将接近奇异点；
[0053]
在判定即将接近奇异点后，机械臂使用避奇异算法，该算法主体为速度势场算法寻找吸 引速度和排斥速度，从而计算避奇异轨迹点，直至生成的轨迹点的雅可比矩阵条件数满足远 离奇异点状态且轨迹点重新回到原本笛卡尔轨迹上；
[0054]
规划避奇异轨迹后，对规划的避奇异轨迹进行平滑处理以及轨迹插值。
[0055]
本发明的奇异点通用规避方法，具体步骤如下：
[0056]
首先以机械臂某关节轴建立参考坐标系，根据参考坐标系使用旋量理论计算各关节轴方 向向量和位置向量
[0057]
使用旋量法计算机械臂雅可比矩阵，将各关节轴方向向量和位置向量带入雅可比 矩阵，雅可比矩阵j(θ)计算公式如下：
[0058]
j(θ)＝[$
1 $
2 $3ꢀ…ꢀ
$
n
]；
[0059]
其中
[0060]
笛卡尔轨迹经运动控制模块的运动学变换模块变换为关节轨迹，再经插值，使用轨 迹点的关节角度值θ求得条件数c，条件数公式如下：
[0061][0062]
在某轨迹点处，若对应的条件数为减小趋势，且出处于奇异影响阈值c0和安全阈值c_sec 之间，则判断机械臂运动正接近奇异点，采用避奇异规划算法规划避奇异轨迹，并将该轨迹 点记录为避奇异轨迹的起始点；
[0063]
避奇异轨迹采用速度势场算法计算排斥速度与吸引速度。当条件数c为减小趋势时，即 接近奇异点过程。以奇异点构成的一排斥势场，将排斥势场映射到关节空间中，求得关节空 间中关节j排斥速度v
rep
为：
[0064][0065]
其中k为斥力系数，c_sec为安全阈值，c0为奇异影响阈值，c
θj
′
为条件数c对于关节值θ
j
偏 导数
[0066]
其中
[0067]
(θ1，θ2，
…
，θ
n
)为该轨迹关节角度值。
[0068]
将求得关节排斥速度使用雅可比矩阵推导至笛卡尔空间，求得由奇异点引起的笛卡尔空 间排斥速度v
c_rep
。该排斥速度有效避免机械臂过于接近奇异点，保证雅可比矩阵仍可有效使 用，即使用雅可比矩阵进行关节速度和笛卡尔速度变换。
[0069]
吸引速度为笛卡尔空间速度，分为两部分，驱动速度v
dri
和修正速度v
adj
。驱动速度v
dri
与原本笛卡尔空间运动速度v
c
同向，有v
dri
＝v
c
‑
v
c_rep
l
c
，其中l
c
为原本笛卡尔轨迹方向向 量。修正速度v
adj
垂直于原本笛卡尔空间轨迹，有v
adj
＝μδx，其中μ为修正速度系数，δx为 垂直于笛卡尔轨迹的偏移量，修正速度确保机械臂在远离奇异点的安全位置时能修正回原本 笛卡尔轨迹上。再使用雅克比逆矩阵将v
dri
、v
adj
推导至关节空间，与v
rep
(θ
j
)求和，求得最 后关节速度组。使用该组关节速度运行一个时间步长，运动到下一个轨迹点，继续使用速度 势场法求取关节速度组和以及以该组关节速度运动到的轨迹点，直至新生成的轨迹点达到满 足脱离奇异条件，该条件为条件数在安全阈值上且生成的轨迹点重新回到原本笛卡尔轨迹 上。结束避奇异规划算法后，进行轨迹平滑，使用五次b样条曲线对整个轨迹进行平滑处 理，在平滑后的轨迹上进行插值得到最终的输出避奇异轨迹。
[0070]
本发明提供一种系统用于实现上述方法，该系统主要包括：
[0071]
运动控制模块，用于接收客户端控制显示模块发送的指令、机械臂运动解析、机械臂正 逆运动学求解、笛卡尔空间运动规划、轨迹的插补、外围设备的控制；
[0072]
奇异点规避规划模块，用于奇异点规避轨迹的规划计算、所得轨迹的平滑处理；
[0073]
客户端控制显示模块，用于用户编辑控制指令、设置机械臂运动状态，将设定的指令、 目标位置机械臂状态发送到运动控制模块，同时显示机械臂的参数、状态；
[0074]
电机驱动控制模块，用于将运动控制模块发送的每个插值周期内的位移转换成脉冲个 数，并将脉冲个数转换成波形信号反馈给驱动电机，以及电机数据的反馈过程；
[0075]
通信模块，用于客户端控制显示模块与运动控制模块之间的通信，客户端控制显示模块 与外围设备之间的通信，外围设备与运动控制模块之间的通信，运动控制模块与电机驱动模 块之间的通信；
[0076]
其中所述运动控制模块包括：
[0077]
运动解析模块，用于将用户编写的运动控制指令解析为机械臂控制器内部指令；
[0078]
机械臂正逆运动学求解模块，用于机械臂关节角坐标系和机械臂笛卡尔空间坐标系之间 的坐标变换运算；
[0079]
笛卡尔空间运动规划模块，用于机械臂在笛卡尔空间规划末端执行器运动轨迹，并将笛 卡尔空间规划轨迹点发送到机械臂正逆运动学求解模块，转换成关节空间中关节角度值；
[0080]
轨迹插值模块，笛卡尔空间轨迹或在奇异点规避模块得到的运动轨迹，输出到轨迹插值 模块中，轨迹插值模块将整个运动轨迹在关节空间中按照插值周期进行插补，将该段插值周 期内的各轴位移值发送给电机驱动模块；
[0081]
所述的奇异点规避规划模块包括：
[0082]
奇异点判断检测模块，在轨迹插值过程中，实时计算关节轨迹点是否满足接近奇异点条 件，若条件数减小且接近安全，则为接近奇异点；
[0083]
奇异点规避轨迹计算模块，在机械臂运动中接近奇异点时，使用以速度势场算法为主的 规划算法，规划避奇异轨迹点；
[0084]
轨迹平滑模块，用于平滑规划得到的避奇异轨迹，使用五次b样条曲线平滑轨迹并输 出到运动控制模块。
[0085]
本发明的机械臂奇异点的通用规避方法使用旋量法计算机械臂雅可比矩阵；构建雅可比 矩阵后，采用雅可比条件数作为奇异点判断条件；在即将接近奇异点过程中，采用速度势场 法，产生排斥速度和吸引速度，避开奇异点，保证了雅可比矩阵仍可有效使用。
[0086]
关于本发明具体结构需要说明的是，本发明采用的各部件模块相互之间的连接关系是确 定的、可实现的，除实施例中特殊说明的以外，其特定的连接关系可以带来相应的技术效 果，并基于不依赖相应软件程序执行的前提下，解决本发明提出的技术问题，本发明中出现 的部件、模块、具体元器件的型号、连接方式除具体说明的以外，均属于本领域技术人员在 申请日前可以获取到的已公开专利、已公开的期刊论文、或公知常识等现有技术，无需赘 述，使得本案提供的技术方案是清楚、完整、可实现的，并能根据该技术手段重现或获得相 应的实体产品。
[0087]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参 照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以 对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替 换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范 围。