CN108582065A - 基于fpga的六轴机械臂运动方程逆解快速求解的方法 - Google Patents

基于fpga的六轴机械臂运动方程逆解快速求解的方法 Download PDF

Info

Publication number
CN108582065A
CN108582065A CN201810178011.7A CN201810178011A CN108582065A CN 108582065 A CN108582065 A CN 108582065A CN 201810178011 A CN201810178011 A CN 201810178011A CN 108582065 A CN108582065 A CN 108582065A
Authority
CN
China
Prior art keywords
clock
calculations
add
clocks
deposit
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.)
Granted
Application number
CN201810178011.7A
Other languages
English (en)
Other versions
CN108582065B (zh
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 CN201810178011.7A priority Critical patent/CN108582065B/zh
Publication of CN108582065A publication Critical patent/CN108582065A/zh
Application granted granted Critical
Publication of CN108582065B publication Critical patent/CN108582065B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/1602—Programme controls characterised by the control system, structure, architecture

Landscapes

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

Abstract

本发明公开了一种基于FPGA的六轴机械臂运动方程逆解快速求解的方法，该方法通过FPGA直接实现逆解计算，根据重新整合后的计算过程，得到相应关节角。首先通过传感器获取机械臂末端目标的运动坐标，输入FPGA中，利用流水线设计与模块时间复用相结合，仅使用2个浮点乘法器、1个浮点除法器、1个浮点开方模块、1个Cordic模块、1个反正切模块，2个浮点加法器，全并行工作，有效降低运动学方程逆解求解所需算子的时钟延时，快速获得逆解(N个角度值)进行输出。从而大大提高了求解运动学方程逆解的速度，并且降低了系统的成本。

Description

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

Claims (3)

1.基于FPGA的六轴机械臂运动方程逆解快速求解的方法，其特征在于，包括：
通过六轴机械臂末端目标的位姿矩阵，根据逆解求解要求目标的位置信息以及计算过程，对所有计算过程进行拆解后进行去相关，然后重新整合；
通过FPGA直接实现逆解计算，根据重新整合后的计算过程，得到相应关节角。
2.根据权利要求1所述的基于FPGA的六轴机械臂运动方程逆解快速求解的方法，其特征在于：计算过程的去相关和重新整合，具体包括：
步骤一：输入的位姿信息
其中，x,y,z直角坐标表示机械臂的末端位置，n,o,a三个旋转矢量表示机械臂的末端姿态；在第I时钟计算d6*ax、d6*ay，d6表示机械臂第六根轴的长度；在第I+6时钟计算mul_o1+py、mul_o2+px,其中mul_o1、mul_o2分别为第一乘法器、第二乘法器在当前时钟的输出；在I+14时钟计算add_o1/add_o2，其中add_o1、add_o2分别为第一加法器、第二加法器在当前时钟的输出；在I+21时钟计算atan(div_o)，其中div_o为除法器在当前时钟的输出；在I+40时钟寄存atan_o为关节角angle1，其中atan_o为反正切模块在当前时钟的输出；
步骤二：在第I+40时钟计算cordic(angle1)，获得的sin(angle1)和cos(angle1)记为s1，c1；在I+71时钟计算nx*s1、ny*c1；在I+72时钟计算nx*c1、ny*s1；在I+73时钟计算ox*s1、oy*c1；在I+74时钟计算ox*c1、oy*s1；在I+75时钟计算ax*s1、ay*c1；在I+77时钟计算ax*c1、ay*s1；在I+77时钟计算nyc1-nxs1，其中nyc1、nxs1分别表示ny*c1、nx*s1的乘积；在I+78时钟计算-nxc1-nys1；在I+79时钟计算oyc1-oxs1；在I+80时钟计算-oxc1-oys1；在I+81时钟计算ayc1-axs1；在I+82时钟计算-axc1-ays1；
步骤三：在第I+1计算d6*az；在I+7计算-pz-d6az；在I+15时钟寄存add_o1为b；在第I+77时钟计算py*c1、px*s1；在I+83时钟计算pyc1-pxs1；在I+91时钟计算add_o1-d2；在I+95时钟计算d6*ax1；在I+99时钟计算add_o1+mul_o1；在I+101时钟计算b*b；在I+107时钟寄存add_o1为a，计算2d5*b、2a4*a，计算b*b+a4*a4，其中d5为机械臂第五轴的长度，2d5即两倍d5的值，a4为第三第四轴的z方向平移距离，2a4为两倍a4；在第I+108时钟计算2a4*b、2d5*a；在第I+113时钟计算mul_o1+mul_o2；在I+114时钟计算mul_o1-mul_o2；在I+115时钟计算a*a+add_o1；在I+121时钟寄存add_o1为c_2；在I+122时钟计算add_o1/c_2；在I+123时钟计算add_o1/c_2；在I+129时钟寄存div_o为c，计算c*c；在I+130时钟寄存div_o为d，计算d*d；在I+135时钟寄存cc；在I+136时钟计算cc-dd；在I+144时钟计算add_o1+1；在I+152时钟计算add_o1*4；在I+158时钟寄存mul_o1为e；
步骤四：在第I+158时钟计算sqrt(e)；在I+163时钟计算c*d；在I+169时钟计算cd*2、cc*2；在I+175时钟计算sqrt(e)-2cd、2cc+2；在I+183时钟计算add_o1/add_o2；在I+190时钟寄存div_o为c23，计算c23*c；在I+196时钟计算mul_o1+d、d5*c23；在I+197时钟计算a4*c23；在I+202时钟计算b+add_o1；在I+203时钟计算b+mul_o1；在I+204时钟寄存s23，计算a4*s23；在I+205计算d5*s23；在I+210时钟计算add_o1-mul_o1；在I+211时钟计算add_o1-mul_o1；在I+218时钟计算add_o1/d5；在I+219时钟计算add_o1/-d5；在I+225时钟寄存s2；在I+226时钟寄存div_o为c2，计算atan(s2,c2)；在I+227时钟计算atan(s23,c23)；在I+245时钟寄存atan_o为angle2；在I+246时钟计算atan_o-angle2寄存为angle3；；
步骤五：在I+206时钟计算nx1*s23、ny1*c23；在I+207时钟计算ox1*s23、oy1*c23；在I+208时钟计算ax1*s23、ay1*c23；在I+209时钟计算ax1*c23、ay1*s23；在I+212时钟计算nx1s23-ny1c23；在I+213时钟计算ox1s23-oy1c23；在I+214时钟计算ax1s23-ay1c23；在I+215时钟计算ax1c23+ay1s23；在I+220时钟寄存add_o1为nx1；在I+221时钟寄存add_o1为ox3；在I+222时钟寄存add_o1为ax3计算atan(ay3,ax3)；在I+223时钟寄存add_o1为az3；在I+241时钟寄存atan_o为angle4；
步骤六：在I+241时钟计算cordic(angle4)；在I+272时钟寄存cordic_o为s4,c4，计算ay3*s4、ax3*c4；在I+273时钟计算ox3*s4、oy3*c4；在I+274时钟计算ny3*c4、nx3*s4；在I+278时钟计算ay3s4+ax3c4；在I+279时钟计算ox3s4-oy3c4；在I+280时钟计算ny3c4-nx3s4；在I+286时钟计算atan(az3,add_o)；在I+287时钟寄存add_o为s6；在I+280时钟计算atan(s6,add_o)；在I+305时钟寄存atan_o为angle5；在I+307时钟寄存atan_o为angle6；
步骤七：执行完成步骤1～6获得的angle1～6的值记为angle_all1，angle_all1为角度组，包含6个角度，下同；判断angle4是否大于0，若大于零，则angle4赋值为angle4-180°，反之赋值为angle4+180°，然后再次执行步骤六，获得第二组angle1～6的值记为angle_all2；
步骤八：修改步骤四中I+175时钟为计算-sqrt(e)-2cd，其余步骤不变重新执行步骤4～7，获得angle_all3和agnle_all4；
步骤九：判断angle1是否大于0，若大于零，则angle1赋值为angle1-180°，反之赋值为angle1+180°；重新执行步骤1～8，获得angle_all5～8；此时存在八组angle1～6的角度值，与前一次传感器输入角度进行比对，选取最优解。
3.根据权利要求1所述的基于FPGA的六轴机械臂运动方程逆解快速求解的方法，其特征在于：所述的六轴机器臂末端目标的位姿矩阵，扩展为N轴机械臂。
CN201810178011.7A 2018-03-05 2018-03-05 基于fpga的六轴机械臂运动方程逆解求解的方法 Active CN108582065B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810178011.7A CN108582065B (zh) 2018-03-05 2018-03-05 基于fpga的六轴机械臂运动方程逆解求解的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810178011.7A CN108582065B (zh) 2018-03-05 2018-03-05 基于fpga的六轴机械臂运动方程逆解求解的方法

Publications (2)

Publication Number Publication Date
CN108582065A true CN108582065A (zh) 2018-09-28
CN108582065B CN108582065B (zh) 2021-05-04

Family

ID=63625683

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810178011.7A Active CN108582065B (zh) 2018-03-05 2018-03-05 基于fpga的六轴机械臂运动方程逆解求解的方法

Country Status (1)

Country Link
CN (1) CN108582065B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002134583A (ja) * 2000-10-24 2002-05-10 Tokyo Electron Ltd 基板搬送装置
CN103499922A (zh) * 2013-09-16 2014-01-08 北京邮电大学 一种基于fpga的七自由度空间机械臂运动学实时解算方法
CN106003051A (zh) * 2016-07-25 2016-10-12 重庆中科博恩思医疗机器人有限公司 一种基于fpga的七自由度力反馈主操作手控制系统
CN106378778A (zh) * 2016-04-29 2017-02-08 南京航空航天大学 一种采用马达代数求解机械臂运动学的方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002134583A (ja) * 2000-10-24 2002-05-10 Tokyo Electron Ltd 基板搬送装置
CN103499922A (zh) * 2013-09-16 2014-01-08 北京邮电大学 一种基于fpga的七自由度空间机械臂运动学实时解算方法
CN106378778A (zh) * 2016-04-29 2017-02-08 南京航空航天大学 一种采用马达代数求解机械臂运动学的方法
CN106003051A (zh) * 2016-07-25 2016-10-12 重庆中科博恩思医疗机器人有限公司 一种基于fpga的七自由度力反馈主操作手控制系统

Non-Patent Citations (1)

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

Also Published As

Publication number Publication date
CN108582065B (zh) 2021-05-04

Similar Documents

Publication Publication Date Title
CN107030698B (zh) 机器人的逆运动学求解系统
CN108241339A (zh) 仿人机械臂的运动求解和构型控制方法
CN110815189B (zh) 基于混合现实的机器人快速示教系统及方法
CN107263466B (zh) 空间机器人基于二次规划问题的基座无扰控制方法
CN113505434B (zh) 基于气动力数学模型的飞行器设计制造方法及其飞行器
WO2022258054A1 (zh) 重建器械术野中心的控制方法、系统和存储介质
CN113715016B (zh) 一种基于3d视觉的机器人抓取方法、系统、装置及介质
He et al. Kinematics analysis and numerical simulation of a manipulator based on virtual prototyping
CN108527368B (zh) 柔性支撑串联工业机器人作业最优初始位姿确定方法
CN111216136A (zh) 多自由度机械手臂控制系统、方法、存储介质、计算机
CN108638073A (zh) 基于fpga的六轴机械臂点到点定距直线规划实现方法
CN108582065A (zh) 基于fpga的六轴机械臂运动方程逆解快速求解的方法
Avizzano et al. An optimal geometric model for clavels delta robot
CN111531532A (zh) 一种基于旋量理论的机器人攀爬运动速度建模方法
Tarmizi et al. Kinematic and dynamic modeling of a multi-fingered robot hand
Fijany et al. Parallel computation of manipulator inverse dynamics
Xu et al. Calculation of operational domain of virtual maintenance based on convex hull algorithm
Kong et al. A novel approach to deriving the unit-homogeneous Jacobian matrices of mechanisms
CN113103239B (zh) 一种机器人姿态轨迹生成方法、装置及存储介质
Lu et al. Control system design and kinematic analysis for a 4-DOF light weight manipulator
Huang et al. Kinematics analysis of abrasive belt grinding robot for aero-engine blade and its simulation
CN116652968A (zh) 多机械臂协同在线仿真方法、装置、电子设备及存储介质
Wang et al. Research of Robot Teleoperation System Based on Unity3D
JP3594985B2 (ja) ロボットの制御装置
Luan Application Research of Industrial Robot Motion Simulation and Control Under Virtual Reality Technology

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
GR01 Patent grant
GR01 Patent grant