[发明专利]基于FPGA的六轴机械臂点到点定距直线规划实现方法

权利要求书

1.基于FPGA的六轴机械臂点到点定距直线规划实现方法，其特征在于，包括：

步骤一：通过传感器获取机械臂末端目标的运动坐标，使用FPGA正解模块求解得到输入N阶位姿矩阵；

其中，x,y,z直角坐标表示机械臂的末端位置，n,o,a三个旋转矢量表示机械臂的末端姿态；

步骤二：利用FPGA的逆解模块求解N阶姿态矩阵所对应的6个关节角度；

步骤三：利用FPGA通过6个关节角度求解计算伺服驱动器的参数；

步骤四：利用FPGA发送出对应的脉冲控制波形，并保留当前N阶位姿矩阵。

2.根据权利要求1所述的基于FPGA的六轴机械臂点到点定距直线规划实现方法，其特征在于：

正解模块求解得到输入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。

3.根据权利要求1所述的基于FPGA的六轴机械臂点到点定距直线规划实现方法，其特征在于：逆解模块求解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的角度值，与前一次传感器输入角度进行比对，选取最优解。

4.根据权利要求1所述的基于FPGA的六轴机械臂点到点定距直线规划实现方法，其特征在于：利用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时钟；

步骤三：迭代上述两个步骤，根据实时的姿态矩阵的变化，输出响应的指令脉冲数。