本发明涉及超冗余机器人运动规划,尤其涉及一种基于标架化空间曲线的运动规划方法。
背景技术:
1、超冗余机器人由于含有较多的运动冗余度,在狭窄空间探测等领域应用广泛。目前超冗余机器人多采用背脊曲线离散化方法进行运动进行规划。目前采用的基于曲线离散化方法采用曲率和挠率积分的方式计算关节角,该方式需要先计算出背脊曲线方程的曲率和挠率,对于复杂的曲线,计算量非常大,会大大降低离散化效率。
技术实现思路
1、针对现有技术的不足,本发明提供了一种基于标架化空间曲线的运动规划方法,解决了现有超冗余机器人空间运动规划计算量大,进而导致超冗余机器人运动规划不实时的技术问题。
2、为解决上述技术问题,本发明提供了如下技术方案:一种基于标架化空间曲线的运动规划方法,该方法包括以下步骤:
3、s1、构建空间参数曲线c(t)作为超冗余机器人的背脊曲线,并计算背脊曲线起始处的单位切向量、单位法向量及单位副法向量构成空间参数曲线初始处的最小旋转标架的坐标系;
4、s2、根据空间参数曲线初始处的坐标系计算前个连杆长度对应弧长处的单位切向量、单位法向量及单位副法向量构成最小旋转标架;
5、s3、基于第个最小旋转标架以及第个最小旋转标架生成超冗余机器人的运动规划。
6、进一步地,在步骤s1中,具体过程包括以下步骤:
7、s11、将空间参数曲线c(t)的曲线参数t进行离散化,其中间隔为,,则曲线参数t的取值为;
8、s12、根据曲线参数t和间隔计算背脊曲线的弧长;
9、s13、令弧长等于超冗余机器人前个连杆长度,并计算与超冗余机器人前个连杆长度相对应的曲线参数;
10、s14、计算曲线参数处的切向量;
11、s15、计算背脊曲线起始处的单位切向量、单位法向量及单位副法向量。
12、进一步地,在步骤s12中,弧长的计算公式为:
13、;
14、上式中,n指曲线段弧长离散求和间隔数,m表示离散曲线段序号数,表示间隔,表示空间参数曲线c(t)在处对t的导数,指向量取模运算。
15、进一步地,在步骤s13中,具体包括:
16、通过以间隔为积分间隔循环累加离散曲线段的弧长直至弧长等于前个连杆长度,进而确定满足条件的曲线参数。
17、进一步地,在步骤s14中,曲线参数处的切向量的计算公式为:
18、;
19、上式中,为空间参数曲线c(t)对曲线参数的1阶导数。
20、进一步地,在步骤s2中,具体过程包括以下步骤:
21、s21、设已知曲线上距离起始处弧长的点处的最小旋转标架为和曲线上弧长的点处的单位切向量;
22、s22、计算最小旋转标架的单位切向量基于平面为镜像的对称向量,其中平面为点和点的二等分面;
23、s23、将最小旋转标架中的单位副法向量基于平面镜像为对称向量,再将对称向量基于平面镜像对称并计算出单位副法向量,其中平面为点处的对称向量和单位切向量的二等分面;
24、s24、采用右手螺旋法则基于单位切向量和单位副法向量计算出单位法向量;
25、s25、根据单位切向量、单位副法向量和单位法向量构成最小旋转标架。
26、进一步地,在步骤s22中,对称向量的计算公式为:
27、;
28、上式中,指以为起点为终点的位置向量,为单位切向量在曲线上点处关于平面的对称向量。
29、进一步地,在步骤s23中,单位副法向量的计算公式为:
30、;
31、上式中,为关于平面镜像的对称向量,为单位切向量和对称向量的向量差,为构成最小旋转标架姿态矩阵的单位副法向量。
32、进一步地,在步骤s3中,具体过程包括以下步骤:
33、s31、计算第个最小旋转标架以及第个最小旋转标架的姿态差值;
34、s32、基于姿态差值计算超冗余机器人正交关节的关节角;
35、s33、根据关节角生成超冗余机器人的运动规划。
36、借由上述技术方案,本发明提供了一种基于标架化空间曲线的运动规划方法,至少具备以下有益效果:
37、本发明通过计算最小旋转标架进行离散化,避免了曲率挠率的计算,对背脊曲线的形状和种类没有要求,对于超冗余机器人背脊曲线离散化避免了大量的曲率挠率及积分计算,可以提高计算效率。
1.一种基于标架化空间曲线的运动规划方法,其特征在于,该方法包括以下步骤:
2.根据权利要求1所述的运动规划方法,其特征在于:在步骤s1中,具体过程包括以下步骤:
3.根据权利要求2所述的运动规划方法,其特征在于:在步骤s12中,弧长的计算公式为:
4.根据权利要求2所述的运动规划方法,其特征在于:在步骤s13中,具体包括:
5.根据权利要求2所述的运动规划方法,其特征在于:在步骤s14中,曲线参数处的切向量的计算公式为:
6.根据权利要求1所述的运动规划方法,其特征在于:在步骤s2中,具体过程包括以下步骤:
7.根据权利要求6所述的运动规划方法,其特征在于:在步骤s22中,对称向量的计算公式为:
8.根据权利要求6所述的运动规划方法,其特征在于:在步骤s23中,单位副法向量的计算公式为:
9.根据权利要求1所述的运动规划方法,其特征在于:在步骤s3中,具体过程包括以下步骤: