本发明涉及仿人机器人关节轨迹规划控制技术领域，特别涉及一种基于四阶三次b样条函数的机器人轨迹规划方法及仿人机器人平台。
背景技术：
随着服务机器人的发展，国内外各研究机构、大专院校、机器人生产厂家研究制造出适合于生活用途的服务机器人产品。其中仿人机器人作为其中的一个热点尤为受到关注，其中日、美等国的学者在仿人机器人领域开展了广泛和深入的研究，取得了较多的理论和实践成果，实现了从静态步行到动态步行，从离线规划到实时控制，从平地行走到斜坡、阶梯行走，从简单的双足行走机构到具有手臂、头部以及腰部的仿人机器人的发展。
机器人的轨迹规划在机器人的控制中具有重要的作用，其性能对机器人的工作效率、运动平稳性和能量消耗具有决定性意义。在实际的生产应用中要求机器人机械系统在运动过程中应尽量平稳无震动，避免位移、速度、加速度的突变。突变的运动需要极大的动力，而电动机受物理的限制不能提供如此大的能量从而造成机器人关节的磨损，减少使用寿命。为了满足这些要求必须选取合适的方法对机器人的运动轨迹进行规划，既能使机器人各关节运动平稳无震动，又能达到高效节能，快速响应到期望位置的目的。
技术实现要素：
本发明的目的在于克服现有技术中的缺点与不足，提供一种基于四阶三次b样条函数规划关节运动轨迹的方法，生成能够达到稳定运动条件的步态行走。通过采用四阶三次b样条函数构造出仿人机器人的运动轨迹，使得机器人各关节的速度，加速度，加加速度的导数连续，避免各关节舵机出现断续，震动等情况，实现在zmp稳定条件下的规划轨迹动作。本发明是以仿人机器人手臂和腿步各关节在空间中按特定轨迹运动过程中能够保持在zmp稳定条件下的控制方法，通过采用四阶三次b样条函数来规划各个关节的运动轨迹，由关节间的直流电机驱动关节生成运动轨迹并能在机器人zmp稳定条件下做稳定运动。
同时，本发明在进行相关轨迹规划的过程中，搭建一套仿人机器人运动控制系统，完成相应轨迹生成后的动作，验证其规划的轨迹可行性。本发明适用于仿人机器人进行行走、舞蹈表演、人机互动过程中，通过设定好的运动轨迹动作完成特定任务。在建立动作函数前，先根据需要的关节动作轨迹，确定四阶三次b样条函数，密化其轨迹点坐标，根据得到的轨迹点坐标构造关于时间的动作函数，通过仿人机器人运动控制系统控制各关节的驱动电机，完成满足zmp条件下的轨迹规划运动。
一种基于四阶三次b样条函数的机器人轨迹规划方法，其特征在于包含如下步骤：
步骤1：定义一个b样条函数，通过定义的b样条函数推到出b样条函数的数学模型；
步骤2：求解四阶三次b样条函数表达式：通过定义的b样条函数，先假设出四阶三次b样条函数的表达式，通过其边界条件和一阶、二阶、三阶倒数都要在形值点处相连接，可以得出十二个条件来求解出四阶三次b样条函数的参数解，从而得到四阶三次b样条函数的表达式；
步骤3：轨迹密化，首先进行参数的密化，计算出各个插补点对应的参数值u，然后带入到样条参数方程中求插补点的坐标，可以得到密化后的四阶三次b样条函数轨迹曲线，完成仿人机器人各关节运动的规划轨迹生成；
步骤4：搭建仿人机器人运动控制系统，通过搭建仿人机器人各关节的运动控制系统完成各关节生成轨迹的动作函数，实现仿人机器人已规划动作的运动控制；
步骤5：判定生成的规划轨迹在仿人机器人zmp稳定条件下的运动状态，为各个关节编辑到下一个形值点的动作函数，达到形值点坐标后设为初始动作点，继续设计到达下一个形值点的动作函数，直到生成轨迹的最后形值点，判断机器人是否达到zmp稳定。
优选的，一种基于四阶三次b样条函数的机器人轨迹规划方法，其进一步包括以下步骤：
(1)定义一个b样条函数，推导出b样条函数数学模型；
1)b样条函数定义如下：
其中，k＞1，i＝0，1，...，n-k，u为参数；
2)：应用上式可以推导出b样条函数的数学模型：设服务机器人的机械臂在关节空间中有一系列的型值点p1、p2、...、pm，由连续条件以及边界条件可以求出m+2个控制点v0、v1、...、vm+1，整条轨迹被分为m-1段，可以将每一段样条轨迹作为b样条轨迹的一个轨迹区间，每段轨迹的末端是下一段轨迹的始端，在每段样条轨迹的始末处连续，第i段b样条曲线连接pi和pi+1型值点，则可以分段表示出机械臂在关节空间中的b样条形值点轨迹曲线；
(2)：求解四阶三次b样条函数
设第i段曲线由vi-1、vi、vi+1、vi+2四个控制点来控制，设四阶三次均匀b样条函数的表达式为：
θi(u)＝x0(u)vi-1+x1(u)vi+x2(u)vi+1+x3(u)vi+2
式中，u为参数，且0≤u≤1，xi(u)为参数多项式，θi(u)为第i段的b样条曲线，即该段的轨迹曲线，另外，还需要满足柯西关系，即坐标变换后的不变性条件：
x0(u)+x1(u)+x2(u)+x3(u)＝1
xi(u)为三次多项式，根据上式可以求出该多项式的系数为：
根据该多项式系数可以确定第i段的b样条曲线轨迹为：
上式中有m+2个数需要求解，但是根据上面的求解过程知，有m个已知条件，因此要想求解处所有的未知数，还需要两个已知条件，如下：
v1＝v0vm+1＝vm，
则可得：
由上式可以唯一确定一组vi值，由此可以确定三次b样条曲线的轨迹；
(3)：轨迹密化方法
对于四阶三次b样条轨迹密化，首先进行参数的密化，计算出各个插补点对应的参数值u，带入到样条参数方程中求插补点的坐标；
设v(t)为插补速度：
则有：
其中：
可得：
采用泰勒一阶展开得：
将上式(15)代入(16)得：
t为插补周期，通过式(17)的递推公式可以计算出u的值，代入三次样条函数式q(u)＝au3+bu2+cu+d，可以得出q(u(t))，q(u(2t))，···等，即可计算出各插补点的位置坐标；
步骤5：搭建出仿人机器人关节轨迹动作指令系统，根据不同的轨迹编写出不同的动作指令实现规划好的任务动作。
本发明还提供一种基于所述四阶三次b样条函数的机器人轨迹规划方法的仿人机器人平台，根据所规划的轨迹通过直流电机驱动机械臂和下肢各关节完成在zmp条件下的稳定控制运动；
(1)搭建基于四阶三次b样条轨迹函数的仿人机器人运动控制系统，包括智能手机及其所开发的特定动作的app应用软件，树莓派系统及其与之连接的蓝牙通信模块，16路舵机控制器，仿人机器人；
(2)通过手机应用软件发送规划轨迹动作口令经过蓝牙通信传递给树莓派系统调用其相应动作函数，经过串口通信传递给16路舵机控制板，驱动各关节机械臂按规划的形值点动作，达到期望的动作要求，并检验其是否在zmp条件下到达稳定状态。
本发明与现有技术相比，有如下优点和特点：
(1)在仿人机器人系统中，各关节的运动轨迹通常采用直线和圆弧进行插补，对于复杂的轨迹曲线难以用简单的直线和圆弧表示，对这些复杂的轨迹曲线进行插补拟合，必须将轨迹曲线分为很多小段的直线或圆弧，这将导致计算复杂，插补效率低，关节动作缓慢，不能达到控制速度和精度要求。采用三阶b样条函数插补运动轨迹能够降低复杂曲线的插补问题，其速度，加速度，加加速度连续可导，能有效避免轨迹加速度尖角引起的机械关节的摩擦，震动问题。
(2)利用四阶三次b样条函数生成的轨迹形值点编写的动作函数。在仿人机器人控制系统中，由各关节机械臂直流电机驱动做出相应的规划轨迹动作，保证在zmp条件下的稳定运动状态。
附图说明
图1为仿人机器人相邻关节处插补方法；
图2为仿人机器人已规划轨迹的密化方法；
图3为搭建的仿人机器人的相应轨迹规划控制系统。
具体实施方式
本发明的目的在于克服现有技术的缺点与不足，提供一种基于四阶三次b样条函数的机器人轨迹规划方法及仿人机器人平台。
其中，一种基于四阶三次b样条函数的机器人轨迹规划方法，包含如下步骤：
步骤1：定义一个b样条函数，通过定义的b样条函数推到出b样条函数的数学模型；
步骤2：求解四阶三次b样条函数表达式：通过定义的b样条函数，先假设出四阶三次b样条函数的表达式，通过其边界条件和一阶、二阶、三阶倒数都要在形值点出相连接，可以得出十二个条件来求解出四阶三次b样条函数的参数解，从而得到四阶三次b样条函数的表达式；
步骤3：轨迹密化，首先进行参数的密化，计算出各个插补点对应的参数值u，然后带入到样条参数方程中求插补点的坐标，可以得到密化后的四阶三次b样条函数轨迹曲线，完成仿人机器人各关节运动的规划轨迹生成；
步骤4：搭建仿人机器人运动控制系统，通过搭建仿人机器人各关节的运动控制系统完成各关节生成轨迹的动作函数，实现仿人机器人已规划动作的运动控制；
步骤5：判定生成的规划轨迹在仿人机器人zmp稳定条件下的运动状态，为各个关节编辑到下一个形值点的动作函数，达到形值点坐标后设为初始动作点，继续设计到达下一个形值点的动作函数，直到生成轨迹的最后形值点，判断机器人是否达到zmp稳定。
优选的，一种基于四阶三次b样条函数的机器人轨迹规划方法，具体实施步骤如下：
(1)基于四阶三次b样条函数的关节运动轨迹设计。
本发明采用如下步骤实现基于四阶三次b样条函数的关节运动轨迹。
步骤1：定义一个b样条函数，我们可以定义b样条函数如下：
其中，k＞1，i＝0，1，...，n-k，u为参数。
步骤2：通过定义的b样条函数推到出b样条函数的数学模型，设服务机器人的机械臂在关节空间中有一系列的型值点p1、p2、...、pm，由连续条件以及边界条件可以求出m+2个控制点v0、v1、...、vm+1，整条轨迹被分为m-1段，可以将每一段样条轨迹作为b样条轨迹的一个轨迹区间，每段轨迹的末端是下一段轨迹的始端，在每段样条轨迹的始末处连续，第i段b样条曲线连接pi和pi+1型值点。则可以分段表示出机械臂在关节空间中的b样条形值点轨迹曲线。
步骤3：求解四阶三次b样条函数，设第i段曲线由vi-1、vi、vi+1、vi+2四个控制点来控制，设控制点的坐标如下：
vi-1(vi-1，qi-1)，vi(vi，qi)，vi+1(vi+1，qi+1)，vi+2(vi+2，qi+2)(1)
可设四阶三次均匀b样条函数的表达式为：
θi(u)＝x0(u)vi-1+x1(u)vi+x2(u)vi+1+x3(u)vi+2(2)
式中，u为参数，且0≤u≤1，xi(u)为参数多项式，θi(u)为第i段的b样条曲线，即该段的轨迹曲线，该轨迹曲线由第i-1段轨迹的末端和第i+1段轨迹的始端相连接，则相邻两型值点的b样条曲线θi(u)和θi+1(u)分别在u＝0和u＝1处满足下列条件：
θi(1)＝θi+1(0)(3)
将上述两公式合并可以得到：
x0(1)＝x3(0)＝0，x1(1)＝x0(0)，x2(1)＝x1(0)，x3(1)＝x2(0)(4)
两型值点间的b样条曲线的一阶、二阶及三阶导数都要在i-1段的末端以及第i+1段的始端相连接，具体关系如下：
θ′i(1)＝θ′i+1(0)，θ″i(1)＝θ″i+1(0)，θ″′i(1)＝θ″′i+1(0)(5)
由此可得12个条件为：
另外，还需要满足柯西关系，即坐标变换后的不变性条件：
x0(u)+x1(u)+x2(u)+x3(u)＝1(7)
xi(u)为三次多项式，根据上式可以求出该多项式的系数为：
上式(8)中有m+2个数需要求解，但是根据上面的求解过程知，有m个已知条件，因此要想求解处所有的未知数，还需要两个已知条件，如下：
v1＝v0，vm+1＝vm(9)
则可得：
由式(10)可以唯一确定一组vi值，由此可以确定四阶三次b样条曲线的轨迹。
步骤4：轨迹密化方法，对于四阶三次b样条轨迹密化，我们首先进行参数的密化，计算出各个插补点对应的参数值u，带入到样条参数方程中求插补点的坐标。具体参见附图1为仿人机器人相邻关节处插补方法，附图2为仿人机器人已规划轨迹的密化方法。
设v(t)为插补速度：
则有：
其中：
将式(13)带入式(12)中可得：
采用泰勒一阶展开得：
将上式(15)代入(16)得：
t为插补周期，通过式(17)的递推公式可以计算出u的值，代入三次样条函数式
q(u)＝au3+bu2+cu+d(18)
可以得出q(u(t))，q(u(2t))，···等，即可计算出各插补点的位置坐标。
步骤5：搭建出仿人机器人关节轨迹动作指令系统，根据不同的轨迹编写出不同的动作指令实现规划好的任务动作。如完成稳定的步行，下蹲表演，手臂敬礼动作等。
图3为搭建的仿人机器人的相应轨迹规划控制系统，本发明涉及一种仿人机器人关节运动轨迹规划方法，结合手机、树莓派系统、机械臂舵机控制器、仿人机器人，搭建起完整的仿人机器人运动轨迹动作系统。通过智能手机发送动作指令，在树莓派系统上接收该指令，调用相关动作函数，传递该动作函数给16路舵机控制器，驱动机器人各关节执行规划好的轨迹动作。其轨迹规划方法采用四阶三次b样条函数构造运动轨迹，经过轨迹密化，可以得到轨迹各形值点坐标，编辑相应动作函数，驱动关节舵机控制器完成仿人机器人相应动作。
本发明还提供一种基于所述四阶三次b样条函数仿人机器人轨迹规划控制方法的仿人机器人平台，根据所规划的轨迹通过直流电机驱动机械臂和下肢各关节完成在zmp条件下的稳定控制运动；
(1)搭建基于四阶三次b样条轨迹函数的仿人机器人运动控制系统，包括智能手机及其所开发的特定动作的app应用软件，树莓派系统及其与之连接的蓝牙通信模块，16路舵机控制器，仿人机器人；
(2)通过手机应用软件发送规划轨迹动作口令经过蓝牙通信传递给树莓派系统调用其相应动作函数，经过串口通信传递给16路舵机控制板，驱动各关节机械臂按规划的形值点动作，达到期望的动作要求，并检验其是否在zmp条件下到达稳定状态。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。