CN108638073A - 基于fpga的六轴机械臂点到点定距直线规划实现方法 - Google Patents

基于fpga的六轴机械臂点到点定距直线规划实现方法 Download PDF

Info

Publication number
CN108638073A
CN108638073A CN201810587228.3A CN201810587228A CN108638073A CN 108638073 A CN108638073 A CN 108638073A CN 201810587228 A CN201810587228 A CN 201810587228A CN 108638073 A CN108638073 A CN 108638073A
Authority
CN
China
Prior art keywords
clock
calculations
clocks
add
clock calculations
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201810587228.3A
Other languages
English (en)
Inventor
黄继业
陆燕怡
谢尚港
高明煜
杨宇翔
何志伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Dianzi University
Original Assignee
Hangzhou Dianzi University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Dianzi University filed Critical Hangzhou Dianzi University
Priority to CN201810587228.3A priority Critical patent/CN108638073A/zh
Publication of CN108638073A publication Critical patent/CN108638073A/zh
Pending legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种基于FPGA的六轴机械臂点到点定距直线规划实现方法，该方法首先通过传感器获取机械臂末端目标的运动坐标，使用FPGA求解得到输入N阶位姿矩阵；通过正解模块求解机械臂末端的起始矩阵和结束矩阵；利用FPGA计算得到，轨迹运动的总时间和加速部分时间，以及初始位姿矩阵和终点位姿矩阵的四元数；根据变化的位姿矩阵，利用FPGA逆解模块进行位姿矩阵的更新，从而确定输出的指令脉冲数。其中，正解和逆解模块都使用流水线设计与模块时间复用相结合。本发明有效降低运动学方程逆解求解所需算子的时钟延时，快速获得逆解和正解的输出；从而大大提高了点到点定距直线规划的计算速度，并且降低了系统的成本。

Description

基于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为反正切模块在当前时钟的输出；
步骤二：在第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个逆解模块，全并行工作，有效降低机械臂点到点定距直线规划求解所需算子的时钟延时，快速获得矩阵信息进行响应指令脉冲数输出。所以，有效解决了现有技术中计算轨迹规划的指令脉冲数方法存在效率低、成本高、耗时量大的技术问题，进而实现了能够高效、低成本、低耗时的求解运动学方程逆解的技术效果。
尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
以上所揭露的仅为本发明较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所做的同等变化，仍属于本发明所涵盖的范围。

Claims (4)

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时钟；
步骤三：迭代上述两个步骤，根据实时的姿态矩阵的变化，输出响应的指令脉冲数。
CN201810587228.3A 2018-06-08 2018-06-08 基于fpga的六轴机械臂点到点定距直线规划实现方法 Pending CN108638073A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810587228.3A CN108638073A (zh) 2018-06-08 2018-06-08 基于fpga的六轴机械臂点到点定距直线规划实现方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810587228.3A CN108638073A (zh) 2018-06-08 2018-06-08 基于fpga的六轴机械臂点到点定距直线规划实现方法

Publications (1)

Publication Number Publication Date
CN108638073A true CN108638073A (zh) 2018-10-12

Family

ID=63752034

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810587228.3A Pending CN108638073A (zh) 2018-06-08 2018-06-08 基于fpga的六轴机械臂点到点定距直线规划实现方法

Country Status (1)

Country Link
CN (1) CN108638073A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110091329A (zh) * 2019-04-30 2019-08-06 河海大学常州校区 一种双机械臂的协同运动控制方法
CN110501958A (zh) * 2019-09-06 2019-11-26 中国科学院长春光学精密机械与物理研究所 一种六自由度运动平台的控制系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100850971B1 (ko) * 2007-06-13 2008-08-12 고종선 지능형 로봇용 움직임 제어 장치
CN103499922A (zh) * 2013-09-16 2014-01-08 北京邮电大学 一种基于fpga的七自由度空间机械臂运动学实时解算方法
CN107127751A (zh) * 2017-03-21 2017-09-05 宁波韦尔德斯凯勒智能科技有限公司 关节型机械臂驱控一体化控制系统及控制方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100850971B1 (ko) * 2007-06-13 2008-08-12 고종선 지능형 로봇용 움직임 제어 장치
CN103499922A (zh) * 2013-09-16 2014-01-08 北京邮电大学 一种基于fpga的七自由度空间机械臂运动学实时解算方法
CN107127751A (zh) * 2017-03-21 2017-09-05 宁波韦尔德斯凯勒智能科技有限公司 关节型机械臂驱控一体化控制系统及控制方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
徐波: "基于FPGA的6R机械臂轨迹规划与控制系统研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110091329A (zh) * 2019-04-30 2019-08-06 河海大学常州校区 一种双机械臂的协同运动控制方法
CN110501958A (zh) * 2019-09-06 2019-11-26 中国科学院长春光学精密机械与物理研究所 一种六自由度运动平台的控制系统

Similar Documents

Publication Publication Date Title
CN107030698B (zh) 机器人的逆运动学求解系统
US9031699B2 (en) Kinematic predictor for articulated mechanisms
CN109159151A (zh) 一种机械臂空间轨迹跟踪动态补偿方法和系统
EP0060563A1 (en) Industrial articulated robot linear interpolation control device
CN108241339A (zh) 仿人机械臂的运动求解和构型控制方法
CN107263466B (zh) 空间机器人基于二次规划问题的基座无扰控制方法
CN114952868B (zh) 7自由度srs型机械臂控制方法及装置、弹琴机器人
EP2188685A1 (en) Inverse kinematics
CN113505434B (zh) 基于气动力数学模型的飞行器设计制造方法及其飞行器
CN108638073A (zh) 基于fpga的六轴机械臂点到点定距直线规划实现方法
CN109933008A (zh) 一种非实时系统和机器人控制器的双插补方法及装置
CN114347045A (zh) 一种双机械臂协同运动控制方法及系统
CN111216136A (zh) 多自由度机械手臂控制系统、方法、存储介质、计算机
CN112356032B (zh) 一种姿态平滑过渡方法及系统
From et al. A real-time algorithm for determining the optimal paint gun orientation in spray paint applications
CN111531532A (zh) 一种基于旋量理论的机器人攀爬运动速度建模方法
CN108582065A (zh) 基于fpga的六轴机械臂运动方程逆解快速求解的方法
Zhao et al. Model and simulation of the mitsubishi rv-m1 robot using matlab
JP4667794B2 (ja) 数値制御方法、数値制御装置、プログラム及びコンピュータ読み取り可能な記録媒体
CN113103239B (zh) 一种机器人姿态轨迹生成方法、装置及存储介质
CN110757453A (zh) 一种超冗余联动机械臂的运动轨迹控制方法、装置及系统
Heise et al. Quaternions and motion interpolation: A tutorial
JP4667796B2 (ja) 数値制御方法、数値制御装置、プログラム及びコンピュータ読み取り可能な記録媒体
CN112693631B (zh) 飞行器在线序列凸优化中的初始轨迹生成方法及系统
JP3594985B2 (ja) ロボットの制御装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication

Application publication date: 20181012

RJ01 Rejection of invention patent application after publication