发明内容
本发明的实施例提供了一种七轴工业机械臂的逆运动学求解方法,本发明提供了如下方案,包括:
步骤1、将虚拟机械臂末端执行器任务设定的起始点和终止点中间对应的位置离散化,获取离散点;
步骤2、建立第一关节角和第七姿态的坐标原点的平面方程,根据每两个离散点得到第一关节角的变化趋势,根据所述变化趋势得到离散点集合中每一点对应的第一关节角的值;
其中,第一平面坐标系为基座底部坐标点、第零姿态的坐标原点以及第一或第二姿态的坐标原点构成的平面;
步骤3、采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T,根据矩阵变换获取第五或第六姿态的坐标原点,再确定位姿矩阵,根据位姿矩阵求出第三姿态的坐标原点;根据第五或第六姿态的坐标原点和第三姿态的坐标原点的距离方程式,求出第二关节角;
步骤4、建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系,根据所述条件关系求解第四关节角;其中,
所述第四关节角的第一特征角为以第三姿态的坐标原点为顶点,与第四姿态的坐标原点及第五或第六姿态坐标原点形成的夹角;
所述第四关节角的第二特征角以第三姿态的坐标原点为顶点,与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的夹角;
步骤5、由正运动学方程的左右两边的第四列对应相等得到的方程得到关于第一关节角、第二关节角、第四关节角以及第三关节角的两个等式,从而求出第三关节角;
步骤6、在上述步骤1至步骤5求解的关节角已知时,利用正运动学确定的位姿矩阵,根据位姿矩阵得到第四姿态的坐标原点坐标,再求出第四姿态的坐标原点和第七姿态的坐标原点之间的空间距离,再由第四姿态的坐标原点、第五或第六姿态的坐标原点和第七姿态的坐标原点构成的三角形中,求出第六关节角的特征角,根据第六关节角的特征角求解第六关节角;其中,第六关节角的特征角为以第五或第六姿态的坐标原点为顶点,与第四姿态的坐标原点、第七姿态的坐标原点形成的夹角;
步骤7、根据第六关节角的求解结果,结合末端坐标的关系,分情况求解第五关节角和第七关节角。
根据本发明的上述方法,在规划第一关节角之前,包括:
以虚拟机械臂末端执行器的以基座的底部点O为基坐标点,再另外依次确定七个位姿状态对应的空间坐标系的坐标原点,其中第一姿态和第二姿态公用同一个坐标原点为第一或第二姿态的坐标原点,第五姿态和第六姿态公用同一个坐标原点为第五或第六姿态的坐标原点;
根据本发明的上述方法,所述建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系,包括:第四关节角的第一特征角与第四关节轴的变换关系:当第四关节角的第一特征角变小时,该变化量是由于第四关节轴向上俯仰造成,同理当第四关节角的第一特征角变大时,该变化量是由于第四关节轴向下俯仰造成,根据所述变换关系建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系。
根据本发明的上述方法,包括:在建立第一关节角和第七姿态的坐标原点的平面方程之前,
旋转第一关节角,将第七姿态的坐标原点旋转至第一平面坐标系内,使第一关节角和第七姿态的坐标原点处于同一平面。
根据本发明的上述方法,包括:
所述第四关节角的第一特征角通过以第三姿态的坐标原点为顶点,与第四姿态的坐标原点及第五或第六姿态坐标原点形成的直角三角形中的三角函数求解;
所述第四关节角的第二特征角通过以以第三姿态的坐标原点为顶点,与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的三角形的三角函数求解。
根据本发明的上述方法,根据第六关节角的特征角求解第六关节角,包括:第六关节角对应为两个解,具体为第六关节角的特征角的补角的正负值。
根据本发明的上述方法,根据第六关节角的求解结果,结合末端坐标的关系,分情况求解第五关节角和第七关节角,包括:
当第六关节角不为零时,由末端坐标的关系求出第五姿态角,再由末端姿态的关系求出第七姿态角;
当第六关节角为零时,机械臂处于奇异位置,不能唯一确定第五关节角和第七关节角,令第五关节角等于零,则可求出第七关节角,得到第五关节角和第七关节角的一组特解。
由上述本发明的实施例提供的技术方案可以看出,本发明实施例根据每两个离散点得到第一关节角的变化趋势得到离散点集合中每一点对应的第一关节角的值;采用每个离散点的末端位姿矩阵,获取第五或第六姿态的坐标原点,再求出第三姿态的坐标原点,求出第二关节角;根据第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系求解第四关节角;由关于第一关节角、第二关节角、第四关节角的方程以及第三关节角的两个等式,求出第三关节角;获取第四姿态的坐标原点坐标,求出第六关节角的特征角,根据第六关节角的特征角求解第六关节角;根据第六关节角的求解结果,分情况求解第五关节角和第七关节角;具有很好的通用性。计算精度较高,且实时性好,并且具有很好的通用性。
实施例一
以虚拟机械臂末端执行器的基座的底部点O为基坐标点,再另外依次确定七个位姿状态对应的空间坐标系的坐标原点,其中第一姿态和第二姿态公用同一个坐标原点为第一或第二姿态的坐标原点,第五姿态和第六姿态公用同一个坐标原点为第五或第六姿态的坐标原点;
由附图1可以得到一个D-H参数表:
除去第一个和最后一个连杆外,每个连杆两端的轴线各有一条法线,分别为前,后相邻连杆的公共法线。这两法线间的距离即为di。把连杆长度称为ai,αi为连杆扭角,θi为两连杆夹角,即表示为第i个关节角,本实施例中,θ1表示第一关节角、θ2表示第二关节角、θ3表示第三关节角、θ4表示第四关节角、θ5表示第五关节角、θ6表示第六关节角、θ7表示第七关节角。
通过上述D-H参数可以得到第i坐标系相对于第i-1坐标系的转换矩阵
则第七坐标系相对于第0坐标系的转换矩阵如下,
由于是冗余机械臂,七个关节角存在着耦合关系,因此利用几何方法求逆,必须先规划出一个关节角的值。考虑到本发明基于的机械臂的机械构型,关节1和关节3都为旋转轴,这里将第一轴虚拟为冗余轴,首先规划出关节角1在每个离散点的值。
如附图1所示,以基座的底部点O为基坐标点,点O0,A,B,B1,C,D依次对应0,1(2),3,4,5(6),7坐标系的原点位置,0,1(2),3,4,5(6),7坐标系是每个转动关节对应的每个虚拟坐标系,每个坐标系对应一个转动关节,第0坐标系对应基座关节,1~7坐标系分别对应1~7转动关节,点O0,A,B,B1,C,D依次是每个转动关节对应的虚拟坐标系的原点,其中,
第零坐标系的坐标原点对应O0,为第零姿态的坐标原点O0;
第一和第二坐标系的坐标原点重叠对应A,为第一或第二姿态的坐标原点A;
第三坐标系的坐标原点对应B,为第三姿态的坐标原点B;
第四坐标系的坐标原点对应B1,为第四姿态的坐标原点B1;
第五和第六坐标系的坐标原点重叠对应C,为第五或第六姿态的坐标原点C;
第七坐标系的坐标原点对应D,为第七姿态的坐标原点D。
初始末端位姿矩阵T为:
初始末端位姿矩阵T是末端坐标系在基坐标系内的转换矩阵,其中,(nx,ny,nz)表示虚拟机械臂末端坐标系的x轴在基坐标系下的坐标表示,(ox,oy,oz)表示虚拟机械臂末端坐标系的y轴在基坐标系下的坐标表示,
(ax,ay,az)表示虚拟机械臂末端坐标系的z轴在基坐标系下的坐标表示,且矢量(nx,ny,nz),(ox,oy,oz),(ax,ay,az)均已单位化,(px,py,pz)表示虚拟机械臂末端坐标系的原点在基坐标系下坐标;则第七姿态的坐标原点D的坐标即为(px py pz)′。
该实施例提供了一种七轴工业机械臂的逆运动学求解方法的处理流程如图2所示,包括如下的处理步骤:
步骤1、将虚拟机械臂末端执行器任务设定的起始点和终止点中间对应的位置离散化,获取离散点;
步骤2、建立第一关节角和第七姿态的坐标原点的平面方程,根据每两个离散点得到第一关节角的变化趋势,根据所述变化趋势得到离散点集合中每一点对应的第一关节角的值;
具体地,旋转第一关节角,将第七姿态的坐标原点旋转至第一平面坐标系内,使第一关节角和第七姿态的坐标原点处于同一平面,建立第一关节角和第七姿态的坐标原点的平面方程,根据每两个离散点得到第一关节角的变化趋势,根据所述变化趋势得到离散点集合中每一点对应的第一关节角的值;
其中,第一平面坐标系为基座底部坐标点、第零姿态的坐标原点以及第一或第二姿态的坐标原点构成的平面。
如附图1所述,将第七姿态的坐标原点D旋转至第一平面AO0O内,使得第一关节角和第七姿态的坐标原点D处于同一个平面,再利用平面方程得到每个离散点与对应的第一关节角θ1的线性关系,
具体做法是旋转第一关节角,使得第七姿态的坐标原点D在平面AO0O内,而平面AO0O的方程为:
-tan q1x+y=0
再将第七姿态的坐标原点D代入上述方程,即可得出q1=atan2(py,px)
根据每两个离散点得到第一关节角θ1的变化趋势,根据所述变化趋势得到离散点集合中每一点对应的第一关节角θ1的值,即利用该变化趋势得到第一关节角θ1从起始点到末端点中每个离散点对应的θ1的值;具体地,如下:序号0表示初始点,1表示第一个离散点,2表示第二个离散点;q10表示初始点(即第0个点)处使得末端坐标系原点在平面AO0O内所需的第一关节角的值,可以认为理想值,而θ10表示初始点处第一关节角的实际值,q11表示第一个离散点处第一关节角的理想值,因此第一个离散点处第一关节角的实际值θ11就应该由公式θ11=θ10+q11-q10得出,后面的迭代即可;
0 T0→q10θ10
1 T1→q11θ11=θ10+q11-q10
2
……
步骤3、采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T,根据矩阵变换获取第五或第六姿态的坐标原点,再确定位姿矩阵,根据位姿矩阵求出第三姿态的坐标原点;根据第五或第六姿态的坐标原点和第三姿态的坐标原点的距离方程式,求出第二关节角;
具体地,由正运动学得到位姿矩阵等式两边运算得到矩阵变换:其中,采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T,根据矩阵变换获取第五或第六姿态的坐标原点,再通过正运动学得到位姿矩阵采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵,结合位姿矩阵求出第三姿态的坐标原点;根据空间中两点的距离公式建立第五或第六姿态的坐标原点和第三姿态的坐标原点的距离方程式,求出第二关节角。
由正运动学的知识可以得到以下位姿矩阵,等式两边进行一定的运算得到:
采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵,每个离散点的末端位姿矩阵的一到三行第4列可得出第五或第六姿态的坐标原点C点坐标(xc yc zc),再通过正运动学采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵求出第三姿态的坐标原点B的坐标(xb yb zb),利用空间中两点的距离公式可以得出两个方程式,并求出第二关节角θ2;
具体地,公式即为由于以每个离散点的末端位姿矩阵作为初始末端位姿矩阵,每个离散点的初始末端位姿矩阵T已知,且转换矩阵的最后一列已知,则可以求出的最后一列,其前三项即为C点坐标;再由公式可计算出出的最后一列只与θ1,θ2有关,而θ1已知,因此最后一列的前三项(B点坐标)只与θ2有关,因此根据空间中两点间距离公式可以确定|BC|只与θ2有关。另一方面,在直角三角形Rt△BB1C中,直角边|BB1|,|B1C|为杆长已知不变,因此可以求出斜边|BC|,故可以求出第二关节角θ2。
步骤4、建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系,根据所述条件关系求解第四关节角;其中,
第四关节角的第一特征角为以第三姿态的坐标原点为顶点,与第四姿态的坐标原点及第五或第六姿态坐标原点形成的夹角;
第四关节角的第二特征角以第三姿态的坐标原点为顶点,与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的夹角。
其中,建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系,包括:第四关节角的第一特征角与第四关节轴的变换关系:当第四关节角的第一特征角变小时,该变化量是由于第四关节轴向上俯仰造成,同理当第四关节角的第一特征角变大时,该变化量是由于第四关节轴向下俯仰造成,根据所述变换关系建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系。
第四关节角的第一特征角通过以第三姿态的坐标原点为顶点,与第四姿态的坐标原点及第五或第六姿态坐标原点形成的直角三角形中的三角函数求解;
第四关节角的第二特征角通过以以第三姿态的坐标原点为顶点,与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的三角形的三角函数求解。
当机械臂处于初始位型时,θ4==0,从机械构型中可知θ4为俯仰角,第四关节角的第二特征角∠ABC固定不变,当关节轴4向上或者向下俯仰时,第四关节角的第一特征角∠B1BC的大小一直在变化,当第四关节角的第一特征角∠B1BC变小时,该变化量是由于关节轴4向上俯仰造成,同理当第四关节角的第一特征角∠B1BC变大时,该变化量是由于关节轴4向下俯仰造成。利用该条件以及关节角极限限制可以得出θ4,∠B1BC,∠ABC三者之间的关系。且以第三姿态的坐标原点为顶点,与第四姿态的坐标原点及第五或第六姿态坐标原点形成的直角三角形RtΔB1BC∠B1BC=atan(B1C/B1B);
在ΔABC中,由于|AB|,|BC|,|AC|均已知,根据三角形余弦定理可得∠ABC。
步骤5、由正运动学方程的左右两边的第四列对应相等得到的方程得到关于第一关节角θ1、第二关节角θ2、第四关节角θ4以及第三关节角θ3的两个等式,从而求出第三关节角θ3;
具体地,由正运动学方程的左右两边的第四列对应相等可得出
方程式连立可以得到关于θ1,θ2,θ4,θ3的两个等式,由于前三个角都已知从而求出θ3;
步骤6、在上述步骤1至步骤5求解的关节角已知时,利用正运动学确定的位姿矩阵,根据位姿矩阵得到第四姿态的坐标原点坐标,再求出第四姿态的坐标原点和第七姿态的坐标原点之间的空间距离,再由第四姿态的坐标原点、第五或第六姿态的坐标原点和第七姿态的坐标原点构成的三角形中,求出第六关节角的特征角,根据第六关节角的特征角求解第六关节角;其中,第六关节角的特征角为以第五或第六姿态的坐标原点为顶点,与第四姿态的坐标原点、第七姿态的坐标原点形成的夹角;
具体地,在上述步骤1至步骤5求解的关节角已知时,利用正运动学确定的位姿矩阵,该矩阵可以得到第四姿态的坐标原点坐标,且第七姿态的坐标原点已知,通过空间中两点之间距离公式,可求出第四姿态的坐标原点和第七姿态的坐标原点之间的距离,在由第四姿态的坐标原点、第五或第六姿态的坐标原点和第七姿态的坐标原点构成的三角形中,根据三条边长,由由余弦定理可求出第六关节角的特征角,根据第六关节角的特征角求解第六关节角;其中,第六关节角的特征角为以第五或第六姿态的坐标原点为顶点,与第四姿态的坐标原点、第七姿态的坐标原点形成的夹角。
根据第六关节角的特征角求解第六关节角,包括:第六关节角对应为两个解,具体为第六关节角的特征角的补角的正负值。
从第一关节角θ1至第四关节角θ4已知时,利用正运动学可确定的位姿矩阵,该矩阵可以得到第四姿态的坐标原点B1点坐标(xB yB zB),且第七姿态的坐标原点D点坐标(pxpy pz)′已知,通过空间中两点之间距离公式,可求出|B1D|,在ΔB1CD中,|B1C|,|CD|均为杆长参数,是已知的,三条边|B1D|,|B1C|,|CD|均已知,故由余弦定理可求出∠B1CD,因此θ6=±(π-∠B1CD)。式中,正,负号对应于θ6的两个解。
步骤7、根据第六关节角的求解结果,结合末端坐标的关系,分情况求解第五关节角和第七关节角。
根据第六关节角的求解结果,结合末端坐标的关系,分情况求解第五关节角和第七关节角,包括:
当第六关节角不为为零时,由末端坐标的关系求出第五姿态角θ5,再由末端姿态的关系求出第七姿态角θ7;
当第六关节角为零时,机械臂处于奇异位置,不能唯一确定第五关节角θ5和第七关节角θ7,令第五关节角θ5=0,则可求出第七关节角θ7,得到第五关节角θ5和第七关节角θ7的一组特解;
具体地,由于第六关节角时,关节5的轴线和关节7的轴线重合,关节5和关节7都是旋转关节,末端执行器的姿态只于θ5+θ7有关,可知θ5和θ7的变化对末端贡献是一样的,相当于失去了一个自由度,处于奇异位形,故可分两种情况来求θ5,θ7。
(1)当第六关节角θ6≠0时,由正运动学方程的结果可知,末端坐标(即D点坐标(px py pz)′)只与θ1~θ6,末端姿态与θ1~θ7有关,而末端位姿矩阵为T已知,故可先由末端坐标的关系求出θ5,再由末端姿态的关系求出θ7;
(2)当第六关节角θ6=0时,机械臂处于奇异位置,这时θ5,θ7不能影响末端坐标,只与末端姿态有关,且θ5,θ7对末端姿态的影响作用相同,故不能唯一确定θ5,θ7,而只能根据末端姿态的关系求出(θ5+θ7),这时可以令θ5=0,则可求出θ7,得到θ5,θ7的一组特解。
通过上述步骤可以求出8组解,这8组解中可能存在复数解或者超出关节极限的解,这部分都应该外加限制进行排除,此时选取哪组角最优,要依据上一个离散点求解出来的关节角来判断,即选取与上一次计算最接近的一组角,这样就可以使得关节位移最小,避免出现关节速度超过电机极限限制。
通过以上的实施方式的描述可知,本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品可以存储在存储介质中,如ROM/RAM、磁碟、光盘等,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。
本说明书中的各个实施例均采用递进的方式描述,各个实施例之间相同相似的部分互相参见即可,每个实施例重点说明的都是与其他实施例的不同之处。尤其,对于装置或系统实施例而言,由于其基本相似于方法实施例,所以描述得比较简单,相关之处参见方法实施例的部分说明即可。以上所描述的装置及系统实施例仅仅是示意性的,其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下,即可以理解并实施。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应该以权利要求的保护范围为准。