[发明专利]一种基于四阶三次B样条函数的机器人轨迹规划方法及仿人机器人平台

权利要求书

1.一种基于四阶三次B样条函数的机器人轨迹规划方法，其特征在于包含如下步骤：

步骤1：定义一个B样条函数，通过定义的B样条函数推到出B样条函数的数学模型；

步骤2：求解四阶三次B样条函数表达式：通过定义的B样条函数，先假设出四阶三次B样条函数的表达式，通过其边界条件和一阶、二阶、三阶倒数都要在形值点处相连接，可以得出十二个条件来求解出四阶三次B样条函数的参数解，从而得到四阶三次B样条函数的表达式；

步骤3：轨迹密化，首先进行参数的密化，计算出各个插补点对应的参数值u，然后带入到样条参数方程中求插补点的坐标，可以得到密化后的四阶三次B样条函数轨迹曲线，完成仿人机器人各关节运动的规划轨迹生成；

步骤4：搭建仿人机器人运动控制系统，通过搭建仿人机器人各关节的运动控制系统完成各关节生成轨迹的动作函数，实现仿人机器人已规划动作的运动控制；

步骤5：判定生成的规划轨迹在仿人机器人ZMP稳定条件下的运动状态，为各个关节编辑到下一个形值点的动作函数，达到形值点坐标后设为初始动作点，继续设计到达下一个形值点的动作函数，直到生成轨迹的最后形值点，判断机器人是否达到ZMP稳定。

2.如权利要求1所述的一种基于四阶三次B样条函数的机器人轨迹规划方法，其特征在于其进一步包括以下步骤：

(1)定义一个B样条函数，推导出B样条函数数学模型；

1)B样条函数定义如下：

其中，k＞1，i＝0，1，...，n-k，u为参数；

2)：应用上式可以推导出B样条函数的数学模型：设服务机器人的机械臂在关节空间中有一系列的型值点P₁、P₂、...、P_m，由连续条件以及边界条件可以求出m+2个控制点V₀、V₁、...、V_m+1，整条轨迹被分为m-1段，可以将每一段样条轨迹作为B样条轨迹的一个轨迹区间，每段轨迹的末端是下一段轨迹的始端，在每段样条轨迹的始末处连续，第i段B样条曲线连接P_i和P_i+1型值点，则可以分段表示出机械臂在关节空间中的B样条形值点轨迹曲线；

(2)：求解四阶三次B样条函数

设第i段曲线由V_i-1、V_i、V_i+1、V_i+2四个控制点来控制，设四阶三次均匀B样条函数的表达式为：

θ_i(u)＝X₀(u)V_i-1+X₁(u)V_i+X₂(u)V_i+1+X₃(u)V_i+2

式中，u为参数，且0≤u≤1，X_i(u)为参数多项式，θ_i(u)为第i段的B样条曲线，即该段的轨迹曲线，另外，还需要满足柯西关系，即坐标变换后的不变性条件：

X₀(u)+X₁(u)+X₂(u)+X₃(u)＝1

X_i(u)为三次多项式，根据上式可以求出该多项式的系数为：

X0(u)=(1-u)36,X1(u)=u32+23-u3X2(u)=-u32+u22+u2+16,X3(u)=u36]]>

根据该多项式系数可以确定第i段的B样条曲线轨迹为：

θi(u)=16[u3,u2,u,1]-13-313-630-30301410Vi-1ViVi+1Vi=2]]>

上式中有m+2个数需要求解，但是根据上面的求解过程知，有m个已知条件，因此要想求解处所有的未知数，还需要两个已知条件，如下：

V₁＝V₀，V_m+1＝V_m

则可得：

6-6141141..................1416-6V0V1V2...VmVm+1=60P1P2...Pm0]]>

由上式可以唯一确定一组V_i值，由此可以确定三次B样条曲线的轨迹；

(3)：轨迹密化方法

对于四阶三次B样条轨迹密化，首先进行参数的密化，计算出各个插补点对应的参数值u，带入到样条参数方程中求插补点的坐标；

设v(t)为插补速度：

v(t)=dsdt=(dsdu)·(dudt)]]>

则有：

dudt=v(t)ds/du]]>

其中：

dsdu=(x)2+(y)2+(z)2]]>

x=Qx(u)y=Qy(u)z=Qz(u)x·=dQx(u)duy·=dQy(u)duz·=dQz(u)du]]>

可得：

dudt=v(t)(x)2+(y)2+(z)2---(15)]]>

采用泰勒一阶展开得：

u((k+1)T)=u(kT)+T·dudt|t=KT---(16)]]>

将上式(15)代入(16)得：

u((k+1)T)=u(KT)+T·v(t)(x)2+(y)2+(z)2|t=KT---(17)]]>

T为插补周期，通过式(17)的递推公式可以计算出u的值，代入三次样条函数式Q(u)＝au³+bu²+cu+d，可以得出Q(u(T))，Q(u(2T))，···等，即可计算出各插补点的位置坐标；

步骤5：搭建出仿人机器人关节轨迹动作指令系统，根据不同的轨迹编写出不同的动作指令实现规划好的任务动作。

3.一种基于权利要求1或2所述的四阶三次B样条函数的机器人轨迹规划方法的仿人机器人平台，根据所规划的轨迹通过直流电机驱动机械臂和下肢各关节完成在ZMP条件下的稳定控制运动；

(1)搭建基于四阶三次B样条轨迹函数的仿人机器人运动控制系统，包括智能手机及其所开发的特定动作的APP应用软件，树莓派系统及其与之连接的蓝牙通信模块，16路舵机控制器，仿人机器人；

(2)通过手机应用软件发送规划轨迹动作口令经过蓝牙通信传递给树莓派系统调用其相应动作函数，经过串口通信传递给16路舵机控制板，驱动各关节机械臂按规划的形值点动作，达到期望的动作要求，并检验其是否在ZMP条件下到达稳定状态。