CN105382835A - 一种可穿越腕部奇异点的机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种可穿越腕部奇异点的机器人路径规划方法，包括以下步骤：操作拖拽示教机器人的末端完成一次完整的作业，采集路径点进行工业机器人的坐标转换和运动学反解，当所述工业机器人当前位姿对应的关节变量值处于腕部奇异区域边界，且原始目标位姿对应的关节变量值在腕关节奇异区域以内时，将机器人5，6轴进行固连，分别根据第三连杆坐标系相对于基坐标系的连杆变换矩阵和工具坐标系相对于连杆坐标系以及工具坐标系相对于基坐标系的连杆变换矩阵之积，计算第三连杆坐标系相对于基坐标系的位置矢量，并据此建立方程组对所述方程组进行求解获得处理后目标位姿的各关节变量，从而获得可通过腕部奇异点准确位置的路径。

Description

一种可穿越腕部奇异点的机器人路径规划方法

技术领域

本发明属于工业机器人路径规划领域，更具体地，涉及一种可穿越腕部奇异点的机器人路径规划方法。

背景技术

工业机器人的路径规划方式主要有3种方式：

1)示教器示教规划：通过示教器上的操纵杆和按钮控制机器人末端运动到指定位置，记录下位姿信息，实现点位示教，再结合简单的直线或圆弧运动指令控制机器人末端简单轨迹运动；

2)引导示教规划：操作人员操作主机器人末端移动要相应位置，将关节角度变化通过位置传感器记录下来直接传递给相同构造的从机器人，使之完成相似运动；

3)离线编程规划：通过离线编程软件生成机器人和作业环境的3D模型，并通过指定模型上的曲面曲线或者点位完成作业路径规划，再发送到机器人控制器中进行示教编程，完成末端复杂轨迹运动。

使用示教器进行路径示教是目前工业环境下最广泛使用的路径规划方式，具有操作简单，存储信息少的特点，但是大部分只能完成直线或者圆弧的末端路径，一旦作业路径变为复杂曲线，该方法就存在着示教点过多，设置耗时以及路径规划进度低的问题，因此无法完成需要进行复杂路径规划的作业。

引导示教的路径规划方法无需复杂操作就可以准确的复现主机器人的运动轨迹，但是因为坐标转换和运动学反解过程中奇异点的存在，要求只有从机器人必须与主机器人具有完全相同的构型，并且基座标位置相同时，运动路径才能复现。而工业现场机器人形式结构多样，要为每种从机器人配置相同结构参数的主机器人则会增加很多成本，局限了主机器人的应用。

基于离线编程路径规划集成功能全面，可以生成复杂路径，但是对操作人员知识水平要求较高，不适应当前国内大部分制造行业生产工人的现状。同时需要对作业场景和机器人生成3D模型，比较费时，同时3D模型也会影响生成路径的精度，因此应用不是很广泛。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种可穿越腕部奇异点的机器人路径规划方法，其目的在于提供一种通过微调末端姿态，直接复现原路径中腕部奇异点准确位置的路径规划方法，由此解决现有的工业机器人在遇到腕部奇异点时不能通过或者成本较高的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种可穿越腕部奇异点的机器人路径规划方法，包括以下步骤：

操作六自由度拖拽示教机器人的末端完成一次完整的作业，将基于该示教机器人基座标的笛卡尔空间坐标信息实时记录，并根据记录将基于示教机器人基座标的路径点空间坐标转换为基于工业机器人基座标的路径点空间坐标；

进行运动学反解，对于工业机器人，当所述工业机器人当前位姿对应的关节变量值处于腕部奇异区域边界且原始目标位姿对应的关节变量值在腕关节奇异区域以内时，则对各关节变量进行以下处理：

将腕关节的5轴和6轴固连，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_{5new}={\mathrm{\θ}}_{5\min }\\ {\mathrm{\θ}}_{6new}={\mathrm{\θ}}_6\end{array}\right.,$

腕关节4轴按照如下参数设定

$\left\{\begin{array}{c}{\mathrm{\δ\θ}}_{4new}={\mathrm{\δ\θ}}_{4old}+\arccos \left(\frac{sin\left({\mathrm{\θ}}_5\right)}{sin\left({\mathrm{\θ}}_{5\min }\right)}\right)\\ {\mathrm{\θ}}_{4new}={\mathrm{\θ}}_{4old}+{\mathrm{\δ\θ}}_{4new}\end{array}\right.$

其中δθ_4old为从当前位姿到达原始目标位姿过程中关节运动所需的第四关节增量值，θ_4old为机器人当前位姿对应的第四关节变量值，θ₅为机器人原始目标位姿对应的第五关节变量值，θ₆为机器人原始目标位姿对应的第六关节变量值，θ_5min为第五关节变量允许的最小值，θ_5new为处理后的目标位姿的第五关节变量值，δθ_4new为机器人从当前位姿运动到处理后的目标位姿对应的第四关节增量值，θ_4new为处理后的目标位姿的第四关节变量值，θ_6new为处理后的目标位姿对应的第六关节变量值；

当路径点P相对于工业机器人基座标的笛卡尔空间位置坐标为：时，分别根据第三连杆坐标系相对于基坐标系的连杆变换矩阵和工具坐标系相对于连杆坐标系以及工具坐标系相对于基坐标系的连杆变换矩阵之积计算第三连杆坐标系相对于基坐标系的位置矢量，并据此建立方程组如下：

$\left\{\begin{array}{c}P_3^0=\left[\begin{array}{c}{a}_2{c}_1{c}_2-s_1{d}_2\\ a_2{s}_1{c}_2+c_1{d}_2\\ -a_2{s}_2\end{array}\right]\\ P_3^0=-R_3^0\left(T_{TCP}^3\right(-{R_{TCP}^0}^{T}P_{TCP}^0\left)\right)\end{array}\right.$

其中，c₁＝cosθ_1new；s₁＝sinθ_1new；a₂分别为连杆1的长度参数，d₂为连杆1，2之间的偏置参数。为末端目标位置相对于工业机器人基座标的笛卡尔空间坐标，表示第三连杆坐标系相对于基座标系的旋转矩阵，表示工具坐标系相对于第三连杆坐标系的齐次变换矩阵，表示工具坐标系相对于基座标系的旋转矩阵；

对所述方程组进行求解获得θ_1new、θ_2new、和θ_3new，其中θ_1new为处理后的目标位姿的第一关节变量值，θ_2new为处理后的目标位姿的第二关节变量值，θ_2new为处理后的目标位姿的第三关节变量值；

根据θ_1new、θ_2new、θ_3new、θ_4new、θ_5new和θ_6new得出通过腕部奇异点的路径。

优选地，所述路径规划方法，其当机器人位姿符合以下条件时判定机器人处于腕部奇异区域：|θ₅|＜θ_5min。

优选地，所述路径规划方法，其对所述方程组进行泰勒展开后，采用牛顿迭代法求解。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

(1)当工业机器人出现腕部奇异点时自动生成奇异点优化路径，通过调整姿态，直接穿越奇异点，实现了位置的准确复现。

(2)采用同一类型的示教机器人可以生成不同构型的六关节串联机器人的优化路径，增加了示教机器人的应用范围，方便示教过程。

附图说明

图1是穿越腕部奇异点的机器人路径规划流程图；

图2是实例中所使用的机器人结构图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明提供的可穿越腕部奇异点的机器人路径规划方法，如图1所示，具体包括以下步骤：

S1：操作人员操作六自由度拖拽示教机器人的末端完成一次完整的作业，将基于该示教机器人基座标的笛卡尔空间坐标信息实时记录在文件中。将基于示教机器人基座标的路径点空间坐标转换为基于工业机器人基座标的路径点空间坐标；

所述将采样点在示教机器人基座标的转换矩阵转换为在工业机器人基座标的转换矩阵的公式如下：

$T_i^A=T_B^{A}T_i^B$

式中，表示示教机器人基座标相对于工业机器人基座标的齐次变换矩阵，表示第i个目标点在示教机器人基座标下的转换矩阵,为第i个目标点在工业机器人基座标的转换矩阵。

S2：对于6自由度机器人进行运动学反解得到原始目标位姿对应的关节变量组，判断是否有位于奇异点区域内的路径点。

θ＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]

S3：如果奇异类型是臂关节和肩关节奇异，则采用奇异方向退化法求出关节变量值来替换原关节变量作为奇异区域内的新关节值；如果根据腕关节奇异的判别条件得出奇异类型为腕关节奇异，则固定5,6关节的关节变量值，将6自由度机器人退化为4自由度机器人，建立几何建模；

设定腕关节奇异的判别条件为：

|θ₅|＜θ_5min。

对于工业机器人，如果所述工业机器人当前位姿对应的关节变量值处于腕部奇异区域边界，且原始目标位姿对应的关节变量值在腕关节奇异区域以内，则需要对关节变量值进行处理，保证处理后的关节变量不在腕部关节奇异区域内。此时各关节变量值对应的机器人处理后的目标位姿虽然发生变化，但末端始终通过目标位置。对各关节变量进行的处理如下：

将腕关节的4，5，6轴及连杆简化为绕4轴旋转的连杆，设定

$\left\{\begin{array}{c}{\mathrm{\θ}}_{5new}={\mathrm{\θ}}_{5\min }\\ {\mathrm{\θ}}_{6new}={\mathrm{\θ}}_6\end{array}\right.$

腕关节4轴按照如下参数设定

$\left\{\begin{array}{c}{\mathrm{\δ\θ}}_{4new}={\mathrm{\δ\θ}}_{4old}+\arccos \left(\frac{sin\left({\mathrm{\θ}}_5\right)}{sin\left({\mathrm{\θ}}_{5\min }\right)}\right)\\ {\mathrm{\θ}}_{4new}={\mathrm{\θ}}_{4old}+{\mathrm{\δ\θ}}_{4new}\end{array}\right.$

其中δθ_4old为从当前位姿到达原始目标位姿过程中关节运动所需的第四关节增量值，θ_4old为机器人当前位姿对应的第四关节变量值，θ₅为机器人原始目标位姿对应的第五关节变量值，θ₆为机器人原始目标位姿对应的第六关节变量值，θ_5min为第五关节变量允许的最小值，θ_5new为处理后的目标位姿的第五关节变量值，δθ_4new为机器人从当前位姿运动到处理后的目标位姿对应的第四关节增量值，θ_4new为处理后的目标位姿的第四关节变量值，θ_6new为处理后的目标位姿对应的第六关节变量值。

S4：由于第六关节坐标系与工具末端坐标系之间有位移和姿态的差别，经过上述奇异点规避方法获得的路径点，其误差会被放大。因此本方法将工具末端坐标系考虑在奇异点路径规划方法内，建立方程组求解出封闭解，获得末端位置无误差的关节变量值；

根据需要到达的点P相对于基座标的笛卡尔空间位置坐标为： $P_{TCP}^0=\[x,y,z\].$

将机器人各关节运动进行分解，分别根据第三连杆坐标系相对于基坐标系的连杆变换矩阵和工具坐标系相对于第三连杆坐标系以及工具坐标系相对于基坐标系的连杆变换矩阵之积计算第三连杆坐标系相对于基坐标系的位置矢量，并据此建立方程组。

根据S3已知θ_4new、θ_5new、θ_6new，TCP工具坐标系相对于腕部坐标系的转换矩阵由标定过程确定，是已知量。已知，则由以下方程组可以求解出θ_1new、θ_2new、θ_3new。θ_1new为处理后的目标位姿的第一关节变量值，θ_2new为处理后的目标位姿的第二关节变量值，θ_2new为处理后的目标位姿的第三关节变量值。

根据第三连杆坐标系相对于基坐标系的连杆变换矩阵计算第三连杆坐标系相对于基坐标系的位置矢量：

$P_3^0=\left[\begin{array}{c}{a}_2{c}_1{c}_2-s_1{d}_2\\ a_2{s}_1{c}_2+c_1{d}_2\\ -a_2{s}_2\end{array}\right]$

根据工具坐标系相对于第三连杆坐标系以及工具坐标系相对于基坐标系的连杆变换矩阵之积计算第三连杆坐标系相对于基坐标系的位置矢量：

$P_3^0=-R_3^0\left(T_{TCP}^3\right(-{R_{TCP}^0}^{T}P_{TCP}^0\left)\right)$

建立方程组如下：

$\left\{\begin{array}{c}P_3^0=\left[\begin{array}{c}{a}_2{c}_1{c}_2-s_1{d}_2\\ a_2{s}_1{c}_2+c_1{d}_2\\ -a_2{s}_2\end{array}\right]\\ P_3^0=-R_3^0\left(T_{TCP}^3\right(-{R_{TCP}^0}^{T}P_{TCP}^0\left)\right)\end{array}\right.$

其中，c₁＝cosθ_1new；s₁＝sinθ_1new；a₂分别为连杆1的长度参数，d₂为连杆1，2之间的偏置参数。为末端目标位置相对于工业机器人基座标的笛卡尔空间坐标，表示第三连杆坐标系相对于基座标系的旋转矩阵，表示工具坐标系相对于第三连杆坐标系的齐次变换矩阵，表示工具坐标系相对于基座标系的旋转矩阵。

对于所述方程组进行求解，具体步骤如下：已知。可通过以下公式得出：

$\left\{\begin{array}{c}R_3^0=\left[\begin{array}{ccc}{c}_1{c}_{23}& -c_1{s}_{23}& -s_1\\ s_1{c}_{23}& -s_1{s}_{23}& c_1\\ -s_{23}& -c_{23}& 0\end{array}\right]\\ T_{TCP}^3=\left[\begin{array}{cc}R_{TCP}^3& P_{TCP}^3\\ \begin{array}{ccc}0& 0& 0\end{array}& 1\end{array}\right]=\left[\begin{array}{cccc}{c}_4{c}_5{c}_6-s_4{s}_6& c_4{c}_5{s}_6-s_4{c}_6& -c_4{s}_5& a_3\\ s_5{c}_6& -s_5{s}_6& c_5& d_4\\ -s_4{c}_5{c}_6-c_4{s}_6& s_4{c}_5{s}_6-c_4{c}_6& s_4{s}_5& 0\\ 0& 0& 0& 1\end{array}\right]T_{TCP}^6\\ R_{TCP}^0=R_3^{0}R_{TCP}^3\end{array}\right.$

上式中，表示第三连杆坐标系相对于基座标系的旋转矩阵，表示工具坐标系相对于第三连杆坐标系的齐次变换矩阵，表示工具坐标系相对于基座标系的旋转矩阵。表示工具坐标系相对于连杆坐标系6的齐次变换矩阵，是已知量。c₁＝cosθ_1new；s₁＝sinθ_1new；c₂₃＝cos(θ_2new+θ_3new)；s₂₃＝sin(θ_2new+θ_3new)，其余依此类推。a₁,a₂,a₃分别为连杆0,1,2的长度参数，d₂,d₄分别为连杆1，2之间的偏置和连杆3,4之间的偏置参数。

通过上述方程进行求解，最终可转化为求解以下三元非线性方程组：

$\left\{\begin{array}{c}{a}_2{c}_1{c}_2-s_1{d}_2=f_1({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\\ a_2{s}_1{c}_2+c_1{d}_2=f_2({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\\ -a_2{s}_2=f_3({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\end{array}\right.$

进而可以转化为：

$\left\{\begin{array}{c}0=f({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})-a_2{c}_1{c}_2+s_1{d}_2\\ 0=f_2({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})-a_2{s}_1{c}_2-c_1{d}_2\\ 0=f_3({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})+a_2{s}_2\end{array}\right.$

从而可以表示为如下方程组：

$\left\{\begin{array}{c}0=G({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\\ 0=M({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\\ 0=K({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\end{array}\right.$

根据多元泰勒公式将方程组进行泰勒展开，再采用牛顿迭代法进行求解。由于关节变量的变化不大，其中θ₁,θ₂,θ₃的迭代初值采用前述反解时计算得到的关节值θ₁,θ₂,θ₃获得。

S5：将生成的关节值导成文件格式，生成机器人执行代码，输入工业机器人进行轨迹控制；

以下为实施例：

以PUMA560结构机器人为例，以下为连杆参数表：

i	ai-1	αi-1	di	θi
					1	0	0°	0	θ1
2	0	-90°	149.09	θ2
					3	431.8	0°	0	θ3
4	20.32	-90°	433.07	θ4
					5	0	90°	0	θ5
6	0	-90°	0	θ6

关节变量5允许的最小值θ_5min为0.5°。

工具坐标系相对于连杆6坐标系的齐次转换矩阵如下：

$T_{TCP}^6=\left[\begin{array}{cccc}\sqrt{2}/2& -\sqrt{2}/2& 0& 10\\ \sqrt{2}/2& \sqrt{2}/2& 0& 0\\ 0& 0& 1& 50\\ 0& 0& 0& 1\end{array}\right]$

取一组数据为例进行计算，当采用基座标变换后，工业机器人当前末端位姿相对于工业机器人基座标的转换矩阵为：

${T_{TCP}^0}_{old}=\left[\begin{array}{cccc}0.9658& -0.2587& 0& -140.431\\ -0.3474& -0.3597& 0.9999& 914.9498\\ -0.2588& -0.9658& -0.0175& 24.4467\\ 0& 0& 0& 1\end{array}\right]$

对当前位姿进行反解，求出对应的关节变量值如下：

θ_old＝[90°,0°,-90°,0°,1°,60°]

目标位姿的转换矩阵为：

$T_{TCP}^0=\left[\begin{array}{cccc}0.7069& -0.7069& 0& -149.09\\ 0& 0& 0.9998& 914.858\\ -0.7069& -0.7069& -0.007& 19.971\\ 0& 0& 0& 1\end{array}\right]$

对原始目标位姿进行反解，求出对应的关节变量值如下：

θ＝[90°,0°,-90°,0°,0.4°,90°]

由腕关节奇异的判别条件可得，目标位姿位于奇异区域以内，需要进行处理使新的目标位姿对应的关节变量在奇异区域以外。

固连5,6轴，则新的目标位姿对应的4,5,6关节变量值如下：

腕关节4轴按照如下参数设定

建立方程组求解1,2,3关节变量值：

$\left[\begin{array}{c}431.8*c_1{c}_2-149.09*s_1\\ 431.8*s_1{c}_2+149.09*c_1\\ -431.8*s_2\end{array}\right]=-R_3^0\left(T_{TCP}^3\right(-{R_{TCP}^0}^{T}P_{TCP}^0\left)\right)$

$\left\{\begin{array}{c}R_3^0=\left[\begin{array}{ccc}{c}_1{c}_{23}& -c_1{s}_{23}& -s_1\\ s_1{c}_{23}& -s_1{s}_{23}& c_1\\ -s_{23}& -c_{23}& 0\end{array}\right]\\ T_{TCP}^3=\left[\begin{array}{cccc}-0.9899& -0.1414& -0.007& 19.9709\\ -0.0062& -0.0062& 1& 483.0681\\ -0.1414& 0.9899& 0.0052& 0.2618\\ 0& 0& 0& 1\end{array}\right]\\ R_{TCP}^0=R_3^0\left[\begin{array}{ccc}-0.9899& -0.1414& -0.007\\ -0.0062& -0.0062& 1\\ -0.1414& 0.9899& 0.0052\end{array}\right]\\ P_{TCP}^0=\left[\begin{array}{c}-149.09\\ 914.858\\ 19.971\end{array}\right]\end{array}\right.$

化简后可得3元非线性方程组：

$\left\{\begin{array}{c}0=G({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\\ 0=M({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\\ 0=K({\mathrm{\θ}}_{1new},{\mathrm{\θ}}_{2new},{\mathrm{\θ}}_{3new})\end{array}\right.$

采用牛顿迭代法进行求解后得到的近似值为：

从而求的新的目标位姿对应的关节变量值。

θ_new＝[89.83°,0°,-90°,36.87°,0.5°,90°]

经运动学正解验算得出得新末端位置与原始末端目标位置一致。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种可穿越腕部奇异点的机器人路径规划方法，其特征在于，包括以下步骤：

操作六自由度拖拽示教机器人的末端完成一次完整的作业，将基于该示教机器人基座标的笛卡尔空间坐标信息实时记录，并根据记录将基于示教机器人基座标的路径点空间坐标转换为基于工业机器人基座标的路径点空间坐标；

进行机器人运动学反解，对于工业机器人，当所述工业机器人当前位姿对应的关节变量值处于腕部奇异区域边界且原始目标位姿对应的关节变量值在腕关节奇异区域以内时，则对各关节变量进行以下处理：

将腕关节的5轴和6轴固连，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_{5new}={\mathrm{\θ}}_{5\min }\\ {\mathrm{\θ}}_{6new}={\mathrm{\θ}}_6\end{array}\right.,$

腕关节4轴按照如下参数设定

$\left\{\begin{array}{c}\mathrm{\δ}{\mathrm{\θ}}_{4new}=\mathrm{\δ}{\mathrm{\θ}}_{4old}+arccos\left(\frac{sin\left({\mathrm{\θ}}_5\right)}{sin\left({\mathrm{\θ}}_{5\min }\right)}\right)\\ {\mathrm{\θ}}_{4new}={\mathrm{\θ}}_{4old}+\mathrm{\δ}{\mathrm{\θ}}_{4new}\end{array}\right.$

其中δθ_4old为从当前位姿到达原始目标位姿过程中关节运动所需的第四关节增量值，θ_4old为机器人当前位姿对应的第四关节变量值，θ₅为机器人原始目标位姿对应的第五关节变量值，θ₆为机器人原始目标位姿对应的第六关节变量值，θ_5min为第五关节变量允许的最小值，θ_5new为处理后的目标位姿的第五关节变量值，δθ_4new为机器人从当前位姿运动到处理后的目标位姿对应的第四关节增量值，θ_4new为处理后的目标位姿的第四关节变量值，θ_6new为处理后的目标位姿对应的第六关节变量值；

当路径点P相对于工业机器人基座标的笛卡尔空间位置坐标为：时，分别根据第三连杆坐标系相对于基坐标系的连杆变换矩阵和工具坐标系相对于连杆坐标系以及工具坐标系相对于基坐标系的连杆变换矩阵之积计算第三连杆坐标系相对于基坐标系的位置矢量，并据此建立方程组如下：

$\left\{\begin{array}{c}P_3^0=\left[\begin{array}{c}{a}_2{c}_1{c}_2-s_1{d}_2\\ a_2{s}_1{c}_2+c_1{d}_2\\ -a_2{s}_2\end{array}\right]\\ P_3^0=-P_3^0\left(T_{TCP}^3\right(-{R_{TCP}^0}^{T}P_{TCP}^0))\end{array}\right.$

其中，c₁＝cosθ_1new；s₁＝sinθ_1new；a₂分别为连杆1的长度参数，d₂为连杆1，2之间的偏置参数。为末端目标位置相对于工业机器人基座标的笛卡尔空间坐标，表示第三连杆坐标系相对于基座标系的旋转矩阵，表示工具坐标系相对于第三连杆坐标系的齐次变换矩阵，表示工具坐标系相对于基座标系的旋转矩阵；

对所述方程组进行求解获得θ_1new、θ_2new、和θ_3new，其中θ_1new为处理后的目标位姿的第一关节变量值，θ_2new为处理后的目标位姿的第二关节变量值，θ_2new为处理后的目标位姿的第三关节变量值；

根据θ_1new、θ_2new、θ_3new、θ_4new、θ_5new和θ_6new得出通过腕部奇异点的路径。

2.如权利要求1所述的可穿越腕部奇异点的机器人路径规划方法，其特征在于，当机器人位姿符合以下条件时判定机器人处于腕部奇异区域：|θ₅|＜θ_5min。

3.如权利要求1所述的可穿越腕部奇异点的机器人路径规划方法，其特征在于，对所述方程组进行泰勒展开后，采用牛顿迭代法求解。