本发明涉及工业机器人运动控制领域，尤其涉及一种基于fpga的六轴机械臂点到点定距直线规划实现方法。
背景技术：
在工业机器人运动控制领域，对实现到将机器人定位到满足工程要求的机器轴位置的方面有着较为广泛的应用；而实现机械臂准确定位定点的关键在于设计并实现一套高效的点到点定距直线规划流程算法，尤其是当机械臂处于高速运动的状态；在传统的技术中，通常将求解六轴机械臂的点到点定距直线规划放在dsp或者上位机上实现，不仅成本高昂而且速度较慢。
技术实现要素：
本发明实施例提供了一种基于fpga的六轴机械臂点到点定距直线规划的实现方法，可以加快求解的速度且降低成本。
为了解决上述技术问题，本发明实施例提供的一种基于fpga的六轴机械臂点到点定距直线规划实现方法，包括：
步骤一：通过传感器获取机械臂末端目标的运动坐标，使用fpga正解模块求解得到输入n阶位姿矩阵；
其中，x,y,z直角坐标表示机械臂的末端位置，n,o,a三个旋转矢量表示机械臂的末端姿态；
步骤二：利用fpga的逆解模块求解n阶姿态矩阵所对应的6个关节角度；
步骤三：利用fpga通过6个关节角度求解计算伺服驱动器的参数；
步骤四：利用fpga发送出对应的脉冲控制波形，并保留当前n阶位姿矩阵。
其中正解模块求解得到输入n阶位姿矩阵过程：
步骤一：在第n时钟，将6个角度值依次送入cordic模块，计算得到6个正弦值和6个余弦值；
步骤二：在第n+t1时钟，依次得到cordic模块的输出的正余弦值，将输出的正余弦值依次送入乘法模块和加法模块计算中间变量,其中t1为cordic模块计算所需时间，为32时钟；
步骤三：在第n+t1+7+2*t2+1*t3时钟依次得到中间变量a,b,c,d；其中t2＝7,t3＝9时钟，t2为乘法器延时，t3为加法器延时；
步骤三：在第n+t1+12+5*t2+3*t3时钟之后，依次每个时钟分别得到nx，ny，nz，ox，oy，oz，ax，ay，az，px,py,pz位姿信息，其中t2＝7,t3＝9时钟，t2为乘法器延时，t3为加法器延时；其中t44直接输出为1。
其中逆解模块求解n阶姿态矩阵所对应的6个关节角度过程：
步骤一：在第n时钟计算d6*ax、d6*ay，d6表示机械臂第六根轴的长度；在第n+6时钟计算mul_o1+py、mul_o2+px,其中mul_o1、mul_o2分别为第一乘法器、第二乘法器在当前时钟的输出；在n+14时钟计算add_o1/add_o2，其中add_o1、add_o2分别为第一加法器、第二加法器在当前时钟的输出；在n+21时钟计算atan(div_o)，其中div_o为除法器在当前时钟的输出；在n+40时钟寄存atan_o为关节角angle1，其中atan_o为反正切模块在当前时钟的输出；
步骤二：在第n+40时钟计算cordic(angle1)，获得的sin(angle1)和cos(angle1)记为s1，c1；在n+71时钟计算nx*s1、ny*c1；在n+72时钟计算nx*c1、ny*s1；在n+73时钟计算ox*s1、oy*c1；在n+74时钟计算ox*c1、oy*s1；在n+75时钟计算ax*s1、ay*c1；在n+77时钟计算ax*c1、ay*s1；在n+77时钟计算nyc1-nxs1，其中nyc1、nxs1分别表示ny*c1、nx*s1的乘积；在n+78时钟计算-nxc1-nys1；在n+79时钟计算oyc1-oxs1；在n+80时钟计算-oxc1-oys1；在n+81时钟计算ayc1-axs1；在n+82时钟计算-axc1-ays1；
步骤三：在第n+1时钟计算d6*az；在n+7时钟计算-pz-d6az；在n+15时钟寄存add_o1为b；在第n+77时钟计算py*c1、px*s1；在n+83时钟计算pyc1-pxs1；在n+91时钟计算add_o1-d2；在n+95时钟计算d6*ax1；在n+99时钟计算add_o1+mul_o1；在n+101时钟计算b*b；在n+107时钟寄存add_o1为a，计算2d5*b、2a4*a，计算b*b+a4*a4，其中d5为机械臂第五轴的长度，2d5即两倍d5的值，a4为第三第四轴的关节偏移，2a4为两倍a4；在第n+108时钟计算2a4*b、2d5*a；在第n+113时钟计算mul_o1+mul_o2；在n+114时钟计算mul_o1-mul_o2；在n+115时钟计算a*a+add_o1；在n+121时钟寄存add_o1为c_2；在n+122时钟计算add_o1/c_2；在n+123时钟计算add_o1/c_2；在n+129时钟寄存div_o为c，计算c*c；在n+130时钟寄存div_o为d，计算d*d；在n+135时钟寄存cc；在n+136时钟计算cc-dd；在n+144时钟计算add_o1+1；在n+152时钟计算add_o1*4；在n+158时钟寄存mul_o1为e；
步骤四：在第n+158时钟计算sqrt(e)；在n+163时钟计算c*d；在n+169时钟计算cd*2、cc*2；在n+175时钟计算sqrt(e)-2cd、2cc+2；在n+183时钟计算add_o1/add_o2；在n+190时钟寄存div_o为c23，计算c23*c；在n+196时钟计算mul_o1+d、d5*c23；在n+197时钟计算a4*c23；在n+202时钟计算b+add_o1；在n+203时钟计算b+mul_o1；在n+204时钟寄存s23，计算a4*s23；在n+205计算d5*s23；在n+210时钟计算add_o1-mul_o1；在n+211时钟计算add_o1-mul_o1；在n+218时钟计算add_o1/d5；在n+219时钟计算add_o1/-d5；在n+225时钟寄存s2；在n+226时钟寄存div_o为c2，计算atan(s2,c2)；在n+227时钟计算atan(s23,c23)；在n+245时钟寄存atan_o为angle2；在n+246时钟计算atan_o-angle2寄存为angle3；；
步骤五(s24)：在n+206时钟计算nx1*s23、ny1*c23；在n+207时钟计算ox1*s23、oy1*c23；在n+208时钟计算ax1*s23、ay1*c23；在n+209时钟计算ax1*c23、ay1*s23；在n+212时钟计算nx1s23-ny1c23；在n+213时钟计算ox1s23-oy1c23；在n+214时钟计算ax1s23-ay1c23；在n+215时钟计算ax1c23+ay1s23；在n+220时钟寄存add_o1为nx1；在n+221时钟寄存add_o1为ox3；在n+222时钟寄存add_o1为ax3计算atan(ay3,ax3)；在n+223时钟寄存add_o1为az3；在n+241时钟寄存atan_o为angle4；
步骤六：在n+241时钟计算cordic(angle4)；在n+272时钟寄存cordic_o为s4,c4，计算ay3*s4、ax3*c4；在n+273时钟计算ox3*s4、oy3*c4；在n+274时钟计算ny3*c4、nx3*s4；在n+278时钟计算ay3s4+ax3c4；在n+279时钟计算ox3s4-oy3c4；在n+280时钟计算ny3c4-nx3s4；在n+286时钟计算atan(az3,add_o)；在n+287时钟寄存add_o为s6；在n+280时钟计算atan(s6,add_o)；在n+305时钟寄存atan_o为angle5；在n+307时钟寄存atan_o为angle6；
步骤七：执行完成步骤一～六获得的angle1～6的值记为angle_all1，angle_all1为角度组，包含6个角度；判断angle4是否大于0，若大于零，则angle4赋值为angle4-180°，反之赋值为angle4+180°，然后再次执行步骤六，获得第二组angle1～6的值记为angle_all2；
步骤八：修改步骤四中n+175时钟为计算-sqrt(e)-2cd，其余步骤不变重新执行步骤四～七，获得angle_all3和agnle_all4；
步骤九：判断angle1是否大于0，若大于零，则angle1赋值为angle1-180°，反之赋值为angle1+180°；重新执行步骤一～八，获得angle_all5～8；此时存在八组angle1～6的角度值，与前一次传感器输入角度进行比对，选取最优解。
其中利用fpga通过6个关节角度求解计算伺服驱动器的参数：
步骤一：在第n时钟，计算dt值；
步骤二：第n+j1+5*j2+7*j3+j4+4*j6+24时钟计算得到姿态，将姿态信息输入逆解模块；其中j1为cordic模块延时，为32时钟；j2为乘法器延时，为7时钟；j3为加法器延时，为9时钟；j4为开方模块延时，为18时钟；j6为除法器延时，为8时钟；第n+j1+5*j2+7*j3+j4+4*j6+24+j7时钟输出逆解的结果，更新角度信息及姿态矩阵,j7为逆解模块延时，延时为1129时钟；
步骤三：迭代上述两个步骤，根据实时的姿态矩阵的变化，输出响应的指令脉冲数。
上述技术方案中，通过传感器获取机械臂末端目标的运动坐标，输入fpga中，利用流水线设计与模块时间复用相结合方法，利用正解，逆解等模块完成点到点定距直线规划过程。其中点到点定距直线规划中的正解和逆解模块皆仅使用2个浮点乘法器、1个浮点除法器、1个浮点开方模块、1个cordic模块、1个反正切模块，全并行工作，有效降低运动学方程求解所需算子的时钟延时，快速获得输出。
本发明实施例可以减少fpga的资源占用量。流水线复用的方法大大减少了时间延时，从而本发明实施例可以减少点到点定距直线规划实现的时间成本。
附图说明
为了更清楚地说明明本发明实施例或现有技术中的技术方案，下面对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创作性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种六轴机械臂点到点定距直线规划快速求解的方法示意图；
图2是本发明实施例提供的一种正解模块求解过程的流程示意图；
图3是本发明实施例提供的一种逆解模块求解过程的流程示意图；
具体实施方式
下面将参考附图来描述本发明所述的基于fpga的六轴机械臂点到点定距直线规划实现方法的实施例。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域的普通技术人员在没有作出创造性劳动前提下所获得的的所有其他实施例，都属于本发明保护范围。
首先，需要了解的是本发明所述的六轴机械臂点到点定距直线规划的快速实现方法，在本实施例中是用于实现高速运动物体的点到点定距直线规划问题。当fpga求解释，其数据将源源不断传输进入处理器。当然，即使是低速运动的物体，也可以将其运动学点到点定距直线规划进行求解。
图1是本发明的六轴机械臂点到点定距直线规划实现方法的流程示意图。其中包括：
步骤一(s1)：通过传感器获取机械臂末端目标的运动坐标，利用fpga的正解模块求解该机械臂的开始和结束位姿矩阵，从而确定轨迹的起点和终点；
步骤二(s2)：利用fpga计算得到，轨迹运动的总时间和加速部分时间，以及初始位姿矩阵和终点位姿矩阵的四元数；
步骤三(s3)：根据变化的位姿矩阵，利用fpga逆解模块进行位姿矩阵的更新，从而确定输出的指令脉冲数。
本发明上述步骤一(s1)包括：
在本实施例中，其步骤具体包括：
步骤一(s10)：在第n时钟，将6个角度值依次送入cordic模块，计算得到6个正弦值和6个余弦值；
步骤二(s11)：在第n+t1时钟，依次得到cordic模块的输出的正余弦值，将该值依次送入乘法模块和加法模块计算中间变量,其中t1为cordic模块计算所需时间，为32时钟；
步骤三(s12)：在第n+t1+7+2*t2+1*t3时钟依次得到中间变量a,b,c,d；其中t2＝7,t3＝9时钟，t2为乘法器延时，t3为加法器延时；
步骤三(s13)：在第n+t1+12+5*t2+3*t3时钟之后，依次每个时钟分别得到t11，t21，t31，t12，t22，t32，t13，t23，t33，t14,t24,t34位姿信息，其中t2＝7,t3＝9时钟，t2为乘法器延时，t3为加法器延时；其中t44直接输出为1。
本发明上述步骤三(s2)包括：
在本实施例中，逆解的过程如图2,3所示，其步骤具体包括：
步骤一(s20)：在第n时钟计算d6*ax、d6*ay，d6表示机械臂第六根轴的长度；在第n+6时钟计算mul_o1+py、mul_o2+px,其中mul_o1、mul_o2分别为第一乘法器、第二乘法器在当前时钟的输出；在n+14时钟计算add_o1/add_o2，其中add_o1、add_o2分别为第一加法器、第二加法器在当前时钟的输出；在n+21时钟计算atan(div_o)，其中div_o为除法器在当前时钟的输出；在n+40时钟寄存atan_o为关节角angle1，其中atan_o为反正切模块在当前时钟的输出。
步骤二(s21)：在第n+40时钟计算cordic(angle1)，获得的sin(angle1)和cos(angle1)记为s1，c1；在n+71时钟计算nx*s1、ny*c1；在n+72时钟计算nx*c1、ny*s1；在n+73时钟计算ox*s1、oy*c1；在n+74时钟计算ox*c1、oy*s1；在n+75时钟计算ax*s1、ay*c1；在n+77时钟计算ax*c1、ay*s1；在n+77时钟计算nyc1-nxs1，其中nyc1、nxs1分别表示ny*c1、nx*s1的乘积；在n+78时钟计算-nxc1-nys1；在n+79时钟计算oyc1-oxs1；在n+80时钟计算-oxc1-oys1；在n+81时钟计算ayc1-axs1；在n+82时钟计算-axc1-ays1；
步骤三(s22)：在第n+1计算d6*az；在n+7计算-pz-d6az；在n+15时钟寄存add_o1为b；在第n+77时钟计算py*c1、px*s1；在n+83时钟计算pyc1-pxs1；在n+91时钟计算add_o1-d2；在n+95时钟计算d6*ax1；在n+99时钟计算add_o1+mul_o1；在n+101时钟计算b*b；在n+107时钟寄存add_o1为a，计算2d5*b、2a4*a，计算b*b+a4*a4，其中d5为机械臂第五轴的长度，2d5即两倍d5的值，a4为第三第四轴的关节偏移，2a4为两倍a4；在第n+108时钟计算2a4*b、2d5*a；在第n+113时钟计算mul_o1+mul_o2；在n+114时钟计算mul_o1-mul_o2；在n+115时钟计算a*a+add_o1；在n+121时钟寄存add_o1为c_2；在n+122时钟计算add_o1/c_2；在n+123时钟计算add_o1/c_2；在n+129时钟寄存div_o为c，计算c*c；在n+130时钟寄存div_o为d，计算d*d；在n+135时钟寄存cc；在n+136时钟计算cc-dd；在n+144时钟计算add_o1+1；在n+152时钟计算add_o1*4；在n+158时钟寄存mul_o1为e。
步骤四(s23)：在第n+158时钟计算sqrt(e)；在n+163时钟计算c*d；在n+169时钟计算cd*2、cc*2；在n+175时钟计算sqrt(e)-2cd、2cc+2；在n+183时钟计算add_o1/add_o2；在n+190时钟寄存div_o为c23，计算c23*c；在n+196时钟计算mul_o1+d、d5*c23；在n+197时钟计算a4*c23；在n+202时钟计算b+add_o1；在n+203时钟计算b+mul_o1；在n+204时钟寄存s23，计算a4*s23；在n+205计算d5*s23；在n+210时钟计算add_o1-mul_o1；在n+211时钟计算add_o1-mul_o1；在n+218时钟计算add_o1/d5；在n+219时钟计算add_o1/-d5；在n+225时钟寄存s2；在n+226时钟寄存div_o为c2，计算atan(s2,c2)；在n+227时钟计算atan(s23,c23)；在n+245时钟寄存atan_o为angle2；在n+246时钟计算atan_o-angle2寄存为angle3；。
步骤五(s24)：在n+206时钟计算nx1*s23、ny1*c23；在n+207时钟计算ox1*s23、oy1*c23；在n+208时钟计算ax1*s23、ay1*c23；在n+209时钟计算ax1*c23、ay1*s23；在n+212时钟计算nx1s23-ny1c23；在n+213时钟计算ox1s23-oy1c23；在n+214时钟计算ax1s23-ay1c23；在n+215时钟计算ax1c23+ay1s23；在n+220时钟寄存add_o1为nx1；在n+221时钟寄存add_o1为ox3；在n+222时钟寄存add_o1为ax3计算atan(ay3,ax3)；在n+223时钟寄存add_o1为az3；在n+241时钟寄存atan_o为angle4。
步骤六(s25)：在n+241时钟计算cordic(angle4)；在n+272时钟寄存cordic_o为s4,c4，计算ay3*s4、ax3*c4；在n+273时钟计算ox3*s4、oy3*c4；在n+274时钟计算ny3*c4、nx3*s4；在n+278时钟计算ay3s4+ax3c4；在n+279时钟计算ox3s4-oy3c4；在n+280时钟计算ny3c4-nx3s4；在n+286时钟计算atan(az3,add_o)；在n+287时钟寄存add_o为s6；在n+280时钟计算atan(s6,add_o)；在n+305时钟寄存atan_o为angle5；在n+307时钟寄存atan_o为angle6。
步骤七(s26)：执行完成步骤1～6获得的angle1～6的值记为angle_all1，angle_all1为角度组，包含6个角度，下同。判断angle4是否大于0，若大于零，则angle4赋值为angle4-180°，反之赋值为angle4+180°，然后再次执行步骤六，获得第二组angle1～6的值记为angle_all2。
步骤八(s27)：修改步骤四中n+175时钟为计算-sqrt(e)-2cd，其余步骤不变重新执行步骤4～7，获得angle_all3和agnle_all4。
步骤九(s28)：判断angle1是否大于0，若大于零，则angle1赋值为angle1-180°，反之赋值为angle1+180°。重新执行步骤1～8，获得angle_all5～8。此时存在八组angle1～6的角度值，与前一次传感器输入角度进行比对，选取最优解。
步骤三(s29)：迭代上述两个步骤，根据实时的姿态矩阵的变化，输出响应的指令脉冲数；
上述，逆解过程使用流水线，合理规划每个计算模块的时钟，使得计算过程得以衔接，大大减小了计算的时间。
上述本申请实施例中的技术方案，至少具有如下的技术效果或优点：利用流水线设计与模块时间复用相结合，仅使用2个浮点加法器、2个浮点乘法器、1个浮点除法器、1个浮点开方模块、1个cordic模块、1个反正切模块，1个正解模块，1个逆解模块，全并行工作，有效降低机械臂点到点定距直线规划求解所需算子的时钟延时，快速获得矩阵信息进行响应指令脉冲数输出。所以，有效解决了现有技术中计算轨迹规划的指令脉冲数方法存在效率低、成本高、耗时量大的技术问题，进而实现了能够高效、低成本、低耗时的求解运动学方程逆解的技术效果。
尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
以上所揭露的仅为本发明较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所做的同等变化，仍属于本发明所涵盖的范围。