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

说明书

本发明公开了一种基于FPGA的六轴机械臂点到点定距直线规划实现方法，该方法首先通过传感器获取机械臂末端目标的运动坐标，使用FPGA求解得到输入N阶位姿矩阵；通过正解模块求解机械臂末端的起始矩阵和结束矩阵；利用FPGA计算得到，轨迹运动的总时间和加速部分时间，以及初始位姿矩阵和终点位姿矩阵的四元数；根据变化的位姿矩阵，利用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为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为反正切模块在当前时钟的输出；