The invention discloses a method for realizing point-to-point distance linear planning of a six-axis manipulator based on FPGA. The method firstly obtains the motion coordinates of the target at the end of the manipulator through a sensor, and then obtains the input N-order pose matrix by using the FPGA. The start matrix and the end matrix at the end of the manipulator are solved by the forward solution module and the end matrix by using the FPGA. The total time of trajectory motion and acceleration part time, the quaternion of the initial pose matrix and the terminal pose matrix are calculated. According to the changed pose matrix, the position matrix is updated by the inverse solution module of FPGA, and the number of output instruction pulses is determined. Among them, both the forward and inverse modules use pipelined design and modular time multiplexing. The invention effectively reduces the clock delay of the operator needed for the inverse solution of the kinematics equation, quickly obtains the output of the inverse solution and the positive solution, thus greatly improving the calculation speed of the point-to-point distance linear programming and reducing the cost of the system.
【技术实现步骤摘要】
基于FPGA的六轴机械臂点到点定距直线规划实现方法
本专利技术涉及工业机器人运动控制领域，尤其涉及一种基于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为cordi...
【技术保护点】
1.基于FPGA的六轴机械臂点到点定距直线规划实现方法，其特征在于，包括：步骤一：通过传感器获取机械臂末端目标的运动坐标，使用FPGA正解模块求解得到输入N阶位姿矩阵；
【技术特征摘要】
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...
【专利技术属性】
技术研发人员：黄继业，陆燕怡，谢尚港，高明煜，杨宇翔，何志伟，
申请(专利权)人：杭州电子科技大学，
类型：发明
国别省市：浙江,33
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。