CN110893621B - 一种智能遥控写字机器人的控制系统的控制方法 - Google Patents
一种智能遥控写字机器人的控制系统的控制方法 Download PDFInfo
- Publication number
- CN110893621B CN110893621B CN201911288229.9A CN201911288229A CN110893621B CN 110893621 B CN110893621 B CN 110893621B CN 201911288229 A CN201911288229 A CN 201911288229A CN 110893621 B CN110893621 B CN 110893621B
- Authority
- CN
- China
- Prior art keywords
- line segment
- float
- module
- axis
- arc
- 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.)
- Active
Links
Images
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
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
-
- 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/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Abstract
本发明公开了一种智能遥控写字机器人的控制系统的控制方法,所述移动终端通过所述蓝牙模块向所述串口通信模块发送信号,并通过串口通信模块将获取的型号发送至所述主控芯片上,通过所述主控芯片获取信号执行相应的指令和算法并将指令发送至电机驱动模块,通过电机驱动模块执行相应的运动。利用本发明的系统和方法,只能遥控写字机器人的写字或画图的精确度高。
Description
技术领域
本发明涉及一种机器人的控制系统的控制方法,特别是涉及一种智能遥控写字机器人的控制系统的控制方法,属于机器人控制技术领域。
背景技术
现在已步入二十一世纪,各种智能算法已逐步进入我们的生活,但是即使在这个电子技术飞速发展的时代,仍然还是免不了在日常生活中需要用到中文进行写作,汉字也是中华名族的基础文字,不过无论是在现在还是将来,学习汉字都是儿童们一个较为枯燥的体验,而如果能把这个过程交与机器人,让机器人来教育儿童进行汉字的书写,这将极大程度上加大儿童对于汉字学习的兴趣,也很大程度能够解放教师的双手,让老师们可以把精力放在其他更有利于青少年儿童全面发展的地方,由此设计一种用于智能遥控写字机器人的控制系统及其控制方法来实现上述问题的优化。
发明内容
本发明的主要目的是为了提供一种智能遥控写字机器人的控制系统的控制方法,利用本发明的方法,只能遥控写字机器人的写字或画图的精确度高。
为达到上述目的,一种智能遥控写字机器人的控制系统的控制方法,控制系统包括包括主控芯片、电机驱动模块、舵机活动模块、串口通信模块、移动终端和蓝牙模块,
所述主控芯片:用于接收和发送信号并进行将获取的信号进行计算分析发出指令;
所述电机驱动模块,用于获取主控芯片发送的控制指令并执行该控制指令;
所述舵机活动模块,用于精准调节写字机器人运动轨迹;
所述串口通信模块,用于通过串口进行通信接收和发送信号;
所述移动终端,用于对所述主控芯片发送控制指令;
所述蓝牙模块,用于移动终端上进行与所述主控芯片短距离无线通信;
所述移动终端通过所述蓝牙模块向所述串口通信模块发送信号,并通过串口通信模块将获取的型号发送至所述主控芯片上,通过所述主控芯片获取信号执行相应的指令和算法并将指令发送至电机驱动模块,通过电机驱动模块执行相应的运动。
所述控制方法包括如下步骤:
步骤1:通过移动终端中的APP将文字、画图或图片指令发送至主控芯片,将指令转换成G代码;
步骤2:通过圆弧插补算法、前瞻算法和DDA算法对获取的指令进行计算并控制电机驱动模块和舵机活动模块;
圆弧插补算法包括如下步骤:
步骤(1):获取圆弧所在平面的圆心坐标o(float center_axis0,float center_axis1)、圆心指向圆弧起始点的向量坐标b(float r_axis0,float r_axis1)和圆心指向圆弧终点的向量坐标c(float rt_axis0,float rt_axis1);
步骤(2):计算圆心到圆弧起始点向量b(float r_axis0 ,float r_axis1)和圆心到圆弧终点向量c (float rt_axis0 ,float rt_axis1)的夹角;
步骤(3):计算圆弧分为线段的数目;计算方法为:设定圆弧的弧度为a,圆弧的半径为r,则圆弧的弧长b=a*r;设定线段AB的两端点到线段对应圆弧的最大距离为h,线段AB长度的一半为j,根据勾股定理有:r*r= j*j+(r-h)*(r-h),已知r和h,则j*j=h*(2*r-h),计算出线段AB的长度=2j,通过b/2j则计算出对应圆弧的线段数目;
步骤(4):利用泰勒公式计算圆心与每条线段对应的cosT和sinT值;
步骤(5):根据当前线段的起始坐标值计算下一线段的起始坐标值,具体方法为:圆的极坐标公式为x=r*cosa,y=r*sina;设定当前线段的起始坐标为(rcosa,rsina),下一条线段比当前线段移动的角度已知为T,那么下一条线段的起始坐标为(rcos(a+T),rsin(a+T)),运算得到rcos(a+T)=r*cosa*cosT-r*sina*sinT,rsin(a+T)=r*sina*cosT+r*cosa*sinT;
步骤(6):计算出下一条线段的起始坐标,也就是当前线段的终点坐标,已知当前线段的起始坐标,这样把线段的坐标传递给直线插补函数进行线段插补;
其中, axis_0,圆弧所在平面的第一个轴,是x/y/z中任意一个;
axis_1,圆弧所在平面的第二个轴,是x/y/z中任意一个;
DDA算法的步骤为:
步骤1):输入线段的坐标值;
步骤2):使m=1=k=b=c/2;其中,m、l和k分别为输入线段向量坐标p(x,y,z)对应的x、y和z的脉冲,c为x,y,z绝对值最大的累加溢出值,b为累加初值;
步骤3):判断预设定值i是否小于c,如果i小于c,则执行步骤4),如果i大于等于c,则执行i=i+1并返回到步骤2);
步骤4):给m、l和k赋值,m=m+x,1=1+y,k=k+z;
步骤5):根据步骤4)中的m、l和k值与c的比较确定给x、y和z轴输出脉冲并驱动写字机器人;
在执行直线插补时,通过前瞻算法对前后线段进行匹配,具体的步骤为:
步骤a:判断缓冲区未满时进行从连续运动段读入新规划元;
步骤b:判断读取不成功时再判断缓冲区是否为空;
步骤c:判断缓冲区不为空时设置最后一个规划元反向规划起始速度为0;
步骤d:判断达到第一个规划元后读取进给速率,设置目标速度约束;
步骤e:计算最大加速度和最大速度、夹角信息;
步骤f:将规划结果送往插补模块,删除当前规划元;
步骤3:通过电机驱动模块和舵机活动模块驱动主轴运动控制和主轴方向控制;
步骤4:获取主轴运动状态并中断定时。
进一步的,所述主控芯片包括USB接口芯片和微控芯片,所述USB接口芯片型号为ATMEGA16U2,所述微控芯片型号为ATMEGA328P-PU,所述电机驱动模块设有两组,所述电机驱动模块采用微步电动机驱动器,且内置转换器。
进一步的,所述主控芯片为下位机,所述移动终端为上位机,所述蓝牙模块HC-05构建无线通信。
进一步的,在步骤(1)中:
float center_axis0 = position[axis_0] + offset[axis_0];float center_axis1 = position[axis_1] + offset[axis_1];
float r_axis0 = -offset[axis_0]; float r_axis1 = -offset[axis_1];float rt_axis0 = target[axis_0] - center_axis0;float rt_axis1 = target[axis_1] - center_axis1;
其中, position,圆弧起始点位置坐标,设为(x0,y0,z0) target,圆弧终点坐标,设为(x1,y1,z1) offset,圆心相对于起始点的偏移向量,设为(rx,ry,yz)。
进一步的,在步骤(2)中,根据向量夹角余弦公式,推出向量夹角正切公式,套入向量b和向量c的坐标,即可得出tanA;下面公式中的atan2是反正切函数,A=atan2(y,x),即夹角A=y/x的正切值;如果圆弧顺时针移动,A应该是负值,如果计算出的A为正值,则在计算出的夹角A基础上减去2*pi;如果圆弧逆时针移动,A应该是正值,如果计算出的A为负值,则在计算出的夹角A基础上加上2*pi,其中,pi为圆周率。
进一步的,在步骤(4)中,采用三角函数的泰勒级数展开公式计算cosT和sinT;cosT的二阶泰勒级数为1-T*T/2,sinT的三阶泰勒级数为T-T*T*T/6, cos_T 被放大了两倍,后面又乘上了0.5进行复原;根据cosT和sinT的泰勒级数公式,cosT被放大了两倍,推出sinT=T*(4+cosT)/6。
进一步的,当步骤4)中的m≥c,则x轴输出脉冲m=m-c,当步骤4中的l≥c,则y轴输出脉冲l=l-c,当步骤4中的k≥c,则z轴输出脉冲k=k-c。
进一步的,所述步骤a中判断缓冲区满时直接设置最后一个规划元反向规划起始速度为0。
进一步的,所述步骤b中读取成功时返回至步骤a中再次判断缓冲区是否满;所述步骤c中判断缓冲区为空时直接结束该算法运行;步骤d中判断未达到第一个规划元时执行反向速度规划,再进行步骤d中的判断是否到达第一个规划元;步骤c中设置目标速度包括目标运行速度和末速度。
本发明的有益技术效果:
本发明提供的一种智能遥控写字机器人的控制系统及其控制方法,通过移动终端控制或者电脑上位机操控进行绘画和仿手写的机器,该机器使用主控芯片与电机驱动模块和舵机模块构成整体的框架,利用实现串口通信,在发出指令后,通过圆弧插补获得更多的小线段,通过获取圆心坐标、圆心到起始点和终止点的向量坐标,利用余弦公式计算两向量的夹角,通过夹角判断移动方向,计算圆弧分为线段的数目,利用泰勒公式计算每一小线段的sinT,cosT值,利用圆的极坐标方程计算线段的起始点和终止点坐标,从而实现对圆弧的分段,并实现每一线段的准确直线运动长度和位置,给直线插补提供了准确的插补数据,以控制写字机器写出更为精确的对应圆弧。通过DDA直线插补提供x、y和z方向的脉冲对电机驱动模块和舵机模块工作,具体的,通过输入线段坐标值,根据i值与c值的比较,从而获得脉冲信号,因此,能精确的在x、y和z中给予相应的脉冲并驱动写字机械人,让写字或绘图更加的精确。并通过前瞻算法判断缓冲区未满时进行从连续运动段读入新规划元,让新规划元读入到缓冲区内,判断读取不成功时再判断缓冲区是否为空,判断缓冲区不为空时设置最后一个规划元反向规划起始速度为0,判断达到第一个规划元后读取进给速率,设置目标速度约束,计算最大加速度和最大速度、夹角信息,将规划结果送往插补模块,删除当前规划元,这样,把当前线段和队列里前一条线段结合在一起,对前一条线段的最大运行速度进行修正,以便保证在前一条线段执行结束时的速度与当前线段的初速度一致。另外,根据两条线段的夹角确定最大转角速度,用于修正前一条线段的结束速度和当前线段的初速度不能超过最大转角速度,使其精确的实现对智能遥控写字机器人运行的控制。
附图说明
图1为按照本发明的一种智能遥控写字机器人的控制系统的一优选实施例的系统图。
图2为按照本发明的一种智能遥控写字机器人的控制方法的一优选实施例的总程序流程图。
图3为按照本发明的一种智能遥控写字机器人的控制方法的一优选实施例的APP流程图。
图4为按照本发明的一种智能遥控写字机器人的控制方法的一优选实施例的圆弧插补算法流程图。
图5为计算线段数目的示意图
图6为按照本发明的一种智能遥控写字机器人的控制方法的一优选实施例的前瞻算法流程图。
图7为按照本发明的一种智能遥控写字机器人的控制方法的一优选实施例的DDA算法流程图。
具体实施方式
为使本领域技术人员更加清楚和明确本发明的技术方案,下面结合实施例及附图对本发明作进一步详细的描述,但本发明的实施方式不限于此。
如图1示,本实施例提供的一种智能遥控写字机器人的控制系统的控制方法,控制系统包括包括主控芯片、电机驱动模块、舵机活动模块、串口通信模块、移动终端和蓝牙模块,所述主控芯片:用于接收和发送信号并进行将获取的信号进行计算分析发出指令;所述电机驱动模块,用于获取主控芯片发送的控制指令并执行该控制指令;所述舵机活动模块,用于精准调节写字机器人运动轨迹;所述串口通信模块,用于通过串口进行无线通信接收和发送信号;所述移动终端,用于对所述主控芯片发送控制指令;所述蓝牙模块,用于移动终端上进行与所述主控芯片短距离无线通信;所述移动终端通过所述蓝牙模块向所述串口通信模块发送信号,并通过串口通信模块将获取的型号发送至所述主控芯片上,通过所述主控芯片获取信号执行相应的指令和算法并将指令发送至电机驱动模块,通过电机驱动模块执行相应的运动。
在本实施例中,所述主控芯片包括USB接口芯片和微控芯片,所述USB接口芯片型号为ATMEGA16U2,所述微控芯片型号为ATMEGA328P-PU,所述电机驱动模块设有两组,所述电机驱动模块采用微步电动机驱动器,且内置转换器。
在本实施例中,所述主控芯片为下位机,所述移动终端为上位机,所述蓝牙模块HC-05构建无线通信。
如图1至图7所示,智能遥控写字机器人的控制方法,包括如下步骤:
步骤1:通过移动终端中的APP将文字、画图或图片指令发送至主控芯片,将指令转换成G代码。具体为:
写字功能:打开手机app连接蓝牙后,在手机命令行串口输入一个汉字,系统会寻找到所预先设定好字库中的汉字相对应的坐标,然后将其发送至主控芯片转换为G代码指令格式。
画图功能:打开手机app连接蓝牙,在手机app画板上进行绘画和涂鸦工作,期间可以撤销和清屏进行修改,点击发送后,app会将所画的路径分解成几个点,相当于点与点进行连接,将指令发送给主控制芯片并将分解的路径转为为G代码格式。
图片打印功能:加载线条分明的图片,可以转换黑白,利用像素点,对其整张图片大小进行一行一行扫描,将图片像素大比例缩小,则可以画出一条直线,进而画出整张图片,将指令发送给主控制芯片并将分解的路径转为为G代码格式。
步骤2:通过圆弧插补算法、前瞻算法和DDA算法对获取的指令进行计算并控制电机驱动模块和舵机活动模块;
圆弧插补算法包括如下步骤:
步骤(1):获取圆弧所在平面的圆心坐标o(float center_axis0,float center_axis1)、圆心指向圆弧起始点的向量坐标b(float r_axis0,float r_axis1)和圆心指向圆弧终点的向量坐标c(float rt_axis0,float rt_axis1);在该步骤中,float center_axis0 = position[axis_0] + offset[axis_0];float center_axis1 = position[axis_1] + offset[axis_1];
float r_axis0 = -offset[axis_0];float r_axis1 = -offset[axis_1];floatrt_axis0 = target[axis_0] - center_axis0;float rt_axis1 = target[axis_1] -center_axis1;
其中, position,圆弧起始点位置坐标,设为(x0,y0,z0) target,圆弧终点坐标,设为(x1,y1,z1) offset,圆心相对于起始点的偏移向量,设为(rx,ry,yz)。
步骤(2):计算圆心到圆弧起始点向量b(float r_axis0 ,float r_axis1)和圆心到圆弧终点向量c (float rt_axis0 ,float rt_axis1)的夹角;在该步骤中,根据向量夹角余弦公式,推出向量夹角正切公式,套入向量b和向量c的坐标,即可得出tanA;下面公式中的atan2是反正切函数,A=atan2(y,x),即夹角A=y/x的正切值;如果圆弧顺时针移动,A应该是负值,如果计算出的A为正值,则在计算出的夹角A基础上减去2*pi;如果圆弧逆时针移动,A应该是正值,如果计算出的A为负值,则在计算出的夹角A基础上加上2*pi,其中,pi为圆周率。
步骤(3):计算圆弧分为线段的数目;计算方法为:设定圆弧的弧度为a,圆弧的半径为r,则圆弧的弧长b=a*r;如图5所示,设定线段AB的两端点到线段对应圆弧的最大距离为h,线段AB长度的一半为j,根据勾股定理有:r*r= j*j+(r-h)*(r-h),已知r和h,则j*j=h*(2*r-h),计算出线段AB的长度=2j,通过b/2j则计算出对应圆弧的线段数目。
步骤(4):利用泰勒公式计算圆心与每条线段对应的cosT和sinT值;在该步骤中,采用三角函数的泰勒级数展开公式计算cosT和sinT;cosT的二阶泰勒级数为1-T*T/2,sinT的三阶泰勒级数为T-T*T*T/6, cos_T 被放大了两倍,后面又乘上了0.5进行复原;根据cosT和sinT的泰勒级数公式,cosT被放大了两倍,推出sinT=T*(4+cosT)/6。
步骤(5):根据当前线段的起始坐标值计算下一线段的起始坐标值,具体方法为:圆的极坐标公式为x=r*cosa,y=r*sina;设定当前线段的起始坐标为(rcosa,rsina),下一条线段比当前线段移动的角度已知为T,那么下一条线段的起始坐标为(rcos(a+T),rsin(a+T)),运算得到rcos(a+T)=r*cosa*cosT-r*sina*sinT,rsin(a+T)=r*sina*cosT+r*cosa*sinT。
步骤(6):计算出下一条线段的起始坐标,也就是当前线段的终点坐标,已知当前线段的起始坐标,这样把线段的坐标传递给直线插补函数进行线段插补。
其中, axis_0,圆弧所在平面的第一个轴,是x/y/z中任意一个;
axis_1,圆弧所在平面的第二个轴,是x/y/z中任意一个;
DDA算法的步骤为:
步骤1):输入线段的坐标值。
步骤2):使m=1=k=b=c/2;其中,m、l和k分别为输入线段向量坐标p(x,y,z)对应的x、y和z的脉冲,c为x,y,z绝对值最大的累加溢出值,b为累加初值。
步骤3):判断预设定值i是否小于c,如果i小于c,则执行步骤4),如果i大于等于c,则执行i=i+1并返回到步骤2)。
步骤4):给m、l和k赋值,m=m+x,1=1+y,k=k+z;在该步骤中,当m≥c,则x轴输出脉冲m=m-c,当l≥c,则y轴输出脉冲l=l-c,当k≥c,则z轴输出脉冲k=k-c。
步骤5):根据步骤4)中的m、l和k值与c的比较确定给x、y和z轴输出脉冲并驱动写字机器人。
在执行直线插补时,通过前瞻算法对前后线段进行匹配,具体的步骤为:
步骤a:判断缓冲区未满时进行从连续运动段读入新规划元。
步骤b:判断读取不成功时再判断缓冲区是否为空。
步骤c:判断缓冲区不为空时设置最后一个规划元反向规划起始速度为0。
步骤d:判断达到第一个规划元后读取进给速率,设置目标速度约束。
步骤e:计算最大加速度和最大速度、夹角信息。
步骤f:将规划结果送往插补模块,删除当前规划元。
步骤3:通过电机驱动模块和舵机活动模块驱动主轴运动控制和主轴方向控制。
所述步骤a中判断缓冲区满时直接设置最后一个规划元反向规划起始速度为0;所述步骤b中读取成功时返回至步骤a中再次判断缓冲区是否满;所述步骤c中判断缓冲区为空时直接结束该算法运行;步骤d中判断未达到第一个规划元时执行反向速度规划,再进行步骤d中的判断是否到达第一个规划元;步骤c中设置目标速度包括目标运行速度和末速度。
步骤4:获取主轴运动状态并中断定时。
本发明提供的一种智能遥控写字机器人的控制系统的控制方法,通过移动终端控制或者电脑上位机操控进行绘画和仿手写的机器,该机器使用主控芯片与电机驱动模块和舵机模块构成整体的框架,利用实现串口通信,在发出指令后,通过圆弧插补获得更多的小线段,通过获取圆心坐标、圆心到起始点和终止点的向量坐标,利用余弦公式计算两向量的夹角,通过夹角判断移动方向,计算圆弧分为线段的数目,利用泰勒公式计算每一小线段的sinT,cosT值,利用圆的极坐标方程计算线段的起始点和终止点坐标,从而实现对圆弧的分段,并实现每一线段的准确直线运动长度和位置,给直线插补提供了准确的插补数据,以控制写字机器写出更为精确的对应圆弧。通过DDA直线插补提供x、y和z方向的脉冲对电机驱动模块和舵机模块工作,具体的,通过输入线段坐标值,根据i值与c值的比较,从而获得脉冲信号,因此,能精确的在x、y和z中给予相应的脉冲并驱动写字机械人,让写字或绘图更加的精确。并通过前瞻算法判断缓冲区未满时进行从连续运动段读入新规划元,让新规划元读入到缓冲区内,判断读取不成功时再判断缓冲区是否为空,判断缓冲区不为空时设置最后一个规划元反向规划起始速度为0,判断达到第一个规划元后读取进给速率,设置目标速度约束,计算最大加速度和最大速度、夹角信息,将规划结果送往插补模块,删除当前规划元,这样,把当前线段和队列里前一条线段结合在一起,对前一条线段的最大运行速度进行修正,以便保证在前一条线段执行结束时的速度与当前线段的初速度一致。另外,根据两条线段的夹角确定最大转角速度,用于修正前一条线段的结束速度和当前线段的初速度不能超过最大转角速度,使其精确的实现对智能遥控写字机器人运行的控制。
以上所述,仅为本发明进一步的实施例,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明所公开的范围内,根据本发明的技术方案及其构思加以等同替换或改变,都属于本发明的保护范围。
Claims (7)
1.一种智能遥控写字机器人的控制系统的控制方法,控制系统包括包括主控芯片、电机驱动模块、舵机活动模块、串口通信模块、移动终端和蓝牙模块,其特征在于:
所述主控芯片:用于接收和发送信号并进行将获取的信号进行计算分析发出指令;
所述电机驱动模块,用于获取主控芯片发送的控制指令并执行该控制指令;
所述舵机活动模块,用于精准调节写字机器人运动轨迹;
所述串口通信模块,用于通过串口进行通信接收和发送信号;
所述移动终端,用于对所述主控芯片发送控制指令;
所述蓝牙模块,用于移动终端上进行与所述主控芯片短距离无线通信;
所述移动终端通过所述蓝牙模块向所述串口通信模块发送信号,并通过串口通信模块将获取的型号发送至所述主控芯片上,通过所述主控芯片获取信号执行相应的指令和算法并将指令发送至电机驱动模块,通过电机驱动模块执行相应的运动;
所述主控芯片包括USB接口芯片和微控芯片,所述USB接口芯片型号为ATMEGA16U2,所述微控芯片型号为ATMEGA328P-PU,所述电机驱动模块设有两组,所述电机驱动模块采用微步电动机驱动器,且内置转换器;
所述主控芯片为下位机,所述移动终端为上位机,所述蓝牙模块构建无线通信;
所述控制方法包括如下步骤:
步骤1:通过移动终端中的APP将文字、画图或图片指令发送至主控芯片,将指令转换成G代码;
步骤2:通过圆弧插补算法、前瞻算法和DDA算法对获取的指令进行计算并控制电机驱动模块和舵机活动模块;
圆弧插补算法包括如下步骤:
步骤(1):获取圆弧所在平面的圆心坐标o(float center_axis0,float center_axis1)、圆心指向圆弧起始点的向量坐标b(float r_axis0,float r_axis1)和圆心指向圆弧终点的向量坐标c(float rt_axis0,float rt_axis1);
步骤(2):计算圆心到圆弧起始点向量b(float r_axis0 ,float r_axis1)和圆心到圆弧终点向量c (float rt_axis0 ,float rt_axis1)的夹角;
步骤(3):计算圆弧分为线段的数目;计算方法为:设定圆弧的弧度为a,圆弧的半径为r,则圆弧的弧长b=a*r;设定线段AB的两端点到线段对应圆弧的最大距离为h,线段AB长度的一半为j,根据勾股定理有:r*r= j*j+(r-h)*(r-h),已知r和h,则j*j=h*(2*r-h),计算出线段AB的长度=2j,通过b/2j则计算出对应圆弧的线段数目;
步骤(4):利用泰勒公式计算圆心与每条线段对应的cosT和sinT值;
步骤(5):根据当前线段的起始坐标值计算下一线段的起始坐标值,具体方法为:圆的极坐标公式为x=r*cosa,y=r*sina;设定当前线段的起始坐标为(rcosa,rsina),下一条线段比当前线段移动的角度已知为T,那么下一条线段的起始坐标为(rcos(a+T),rsin(a+T)),运算得到rcos(a+T)=r*cosa*cosT-r*sina*sinT,rsin(a+T)=r*sina*cosT+r*cosa*sinT;
步骤(6):计算出下一条线段的起始坐标,也就是当前线段的终点坐标,已知当前线段的起始坐标,这样把线段的坐标传递给直线插补函数进行线段插补;
其中, axis_0,圆弧所在平面的第一个轴,是x/y/z中任意一个;
axis_1,圆弧所在平面的第二个轴,是x/y/z中任意一个;
DDA算法的步骤为:
步骤1):输入线段的坐标值;
步骤2):使m=1=k=b=c/2;其中,m、l和k分别为输入线段向量坐标p(x,y,z)对应的x、y和z的脉冲,c为x,y,z绝对值最大的累加溢出值,b为累加初值;
步骤3):判断预设定值i是否小于c,如果i小于c,则执行步骤4),如果i大于等于c,则执行i=i+1并返回到步骤2);
步骤4):给m、l和k赋值,m=m+x,1=1+y,k=k+z;
步骤5):根据步骤4)中的m、l和k值与c的比较确定给x、y和z轴输出脉冲并驱动写字机器人;
在执行直线插补时,通过前瞻算法对前后线段进行匹配,具体的步骤为:
步骤a:判断缓冲区未满时进行从连续运动段读入新规划元;
步骤b:判断读取不成功时再判断缓冲区是否为空;
步骤c:判断缓冲区不为空时设置最后一个规划元反向规划起始速度为0;
步骤d:判断达到第一个规划元后读取进给速率,设置目标速度约束;
步骤e:计算最大加速度和最大速度、夹角信息;
步骤f:将规划结果送往插补模块,删除当前规划元;
步骤3:通过电机驱动模块和舵机活动模块驱动主轴运动控制和主轴方向控制;
步骤4:获取主轴运动状态并中断定时。
2.根据权利要求1所述的一种智能遥控写字机器人的控制系统的控制方法,其特征在于,在步骤(1)中:
float center_axis0 = position[axis_0] + offset[axis_0];float center_axis1= position[axis_1] + offset[axis_1];
float r_axis0 = -offset[axis_0]; float r_axis1 = -offset[axis_1];floatrt_axis0 = target[axis_0] - center_axis0;float rt_axis1 = target[axis_1] -center_axis1;
其中, position,圆弧起始点位置坐标,设为(x0,y0,z0) target,圆弧终点坐标,设为(x1,y1,z1) offset,圆心相对于起始点的偏移向量,设为(rx,ry,yz)。
3.根据权利要求1所述的一种智能遥控写字机器人的控制系统的控制方法,其特征在于:在步骤(2)中,根据向量夹角余弦公式,推出向量夹角正切公式,套入向量b和向量c的坐标,即可得出tanA;下面公式中的atan2是反正切函数,A=atan2(y,x),即夹角A=y/x的正切值;如果圆弧顺时针移动,A应该是负值,如果计算出的A为正值,则在计算出的夹角A基础上减去2*pi;如果圆弧逆时针移动,A应该是正值,如果计算出的A为负值,则在计算出的夹角A基础上加上2*pi,其中,pi为圆周率。
4.根据权利要求1所述的一种智能遥控写字机器人的控制系统的控制方法,其特征在于:在步骤(4)中,采用三角函数的泰勒级数展开公式计算cosT和sinT;cosT的二阶泰勒级数为1-T*T/2,sinT的三阶泰勒级数为T-T*T*T/6, cos_T 被放大了两倍,后面又乘上了0.5进行复原;根据cosT和sinT的泰勒级数公式,cosT被放大了两倍,推出sinT=T*(4+cosT)/6。
5.根据权利要求1所述的一种智能遥控写字机器人的控制系统的控制方法,其特征在于,当步骤4)中的m≥c,则x轴输出脉冲m=m-c,当步骤4)中的l≥c,则y轴输出脉冲l=l-c,当步骤4)中的k≥c,则z轴输出脉冲k=k-c。
6.根据权利要求1所述的一种智能遥控写字机器人的控制系统的控制方法,其特征在于,所述步骤a中判断缓冲区满时直接设置最后一个规划元反向规划起始速度为0。
7.根据权利要求1所述的一种智能遥控写字机器人的控制系统的控制方法,其特征在于,所述步骤b中读取成功时返回至步骤a中再次判断缓冲区是否满;所述步骤c中判断缓冲区为空时直接结束该算法运行;步骤d中判断未达到第一个规划元时执行反向速度规划,再进行步骤d中的判断是否到达第一个规划元;步骤c中设置目标速度包括目标运行速度和末速度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911288229.9A CN110893621B (zh) | 2019-12-16 | 2019-12-16 | 一种智能遥控写字机器人的控制系统的控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911288229.9A CN110893621B (zh) | 2019-12-16 | 2019-12-16 | 一种智能遥控写字机器人的控制系统的控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110893621A CN110893621A (zh) | 2020-03-20 |
CN110893621B true CN110893621B (zh) | 2021-07-30 |
Family
ID=69788529
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911288229.9A Active CN110893621B (zh) | 2019-12-16 | 2019-12-16 | 一种智能遥控写字机器人的控制系统的控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110893621B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113759806B (zh) * | 2021-09-28 | 2023-06-30 | 广西科技师范学院 | 一种写字机器人控制系统 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN2654293Y (zh) * | 2003-11-10 | 2004-11-10 | 重庆大学 | 写字机器人 |
CN103317507A (zh) * | 2013-06-07 | 2013-09-25 | 浙江工业大学 | 一种机械手运动控制器的中文示教方法 |
CN103358309A (zh) * | 2013-07-12 | 2013-10-23 | 杭州金人自动控制设备有限公司 | 一种基于以太网的机械手运动控制系统 |
CN104191428A (zh) * | 2014-08-27 | 2014-12-10 | 深圳科瑞技术股份有限公司 | 一种基于scara机械手的运动轨迹规划方法及系统 |
CN104308847A (zh) * | 2014-11-19 | 2015-01-28 | 天津理工大学 | 一种基于物联网的时钟机器人系统及工作方法 |
CN105269556A (zh) * | 2015-09-20 | 2016-01-27 | 深圳市得意自动化科技有限公司 | 机器人的运动前瞻方法 |
CN106166741A (zh) * | 2016-05-27 | 2016-11-30 | 宁波创控智能科技有限公司 | 高精度智能写字机器人 |
CN207189676U (zh) * | 2017-09-20 | 2018-04-06 | 武汉需要智能技术有限公司 | 一种写字机器人 |
CN209466253U (zh) * | 2018-12-10 | 2019-10-08 | 潍坊职业学院 | 一种写字机器人 |
-
2019
- 2019-12-16 CN CN201911288229.9A patent/CN110893621B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN2654293Y (zh) * | 2003-11-10 | 2004-11-10 | 重庆大学 | 写字机器人 |
CN103317507A (zh) * | 2013-06-07 | 2013-09-25 | 浙江工业大学 | 一种机械手运动控制器的中文示教方法 |
CN103358309A (zh) * | 2013-07-12 | 2013-10-23 | 杭州金人自动控制设备有限公司 | 一种基于以太网的机械手运动控制系统 |
CN104191428A (zh) * | 2014-08-27 | 2014-12-10 | 深圳科瑞技术股份有限公司 | 一种基于scara机械手的运动轨迹规划方法及系统 |
CN104308847A (zh) * | 2014-11-19 | 2015-01-28 | 天津理工大学 | 一种基于物联网的时钟机器人系统及工作方法 |
CN105269556A (zh) * | 2015-09-20 | 2016-01-27 | 深圳市得意自动化科技有限公司 | 机器人的运动前瞻方法 |
CN106166741A (zh) * | 2016-05-27 | 2016-11-30 | 宁波创控智能科技有限公司 | 高精度智能写字机器人 |
CN207189676U (zh) * | 2017-09-20 | 2018-04-06 | 武汉需要智能技术有限公司 | 一种写字机器人 |
CN209466253U (zh) * | 2018-12-10 | 2019-10-08 | 潍坊职业学院 | 一种写字机器人 |
Also Published As
Publication number | Publication date |
---|---|
CN110893621A (zh) | 2020-03-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110125944B (zh) | 一种机械臂示教系统和方法 | |
CN102810008A (zh) | 一种空中输入系统、方法及空中输入采集设备 | |
CN103167338A (zh) | 一种基于移动终端的智能电视输入控制系统及方法 | |
CN206170098U (zh) | 一种可室内跟随目标的自动取物机器人 | |
CN104133424A (zh) | 一种基于可编程控制器的工业机器人直线插补方法 | |
CN110893621B (zh) | 一种智能遥控写字机器人的控制系统的控制方法 | |
CN103737592A (zh) | 一种机械手精确控制系统及其方法 | |
CN101576954A (zh) | 书写笔画识别装置、移动终端及实现空间书写的方法 | |
CN103712619B (zh) | 一种基于航迹仪的高精度标绘控制方法 | |
CN102981504A (zh) | 基于Zigbee通信的多机器人队形控制系统及方法 | |
CN102135823A (zh) | 一种智能电子手写笔 | |
CN104598048A (zh) | 数位笔书写控制方法及系统 | |
CN111113415B (zh) | 一种基于二维码路标、摄像头和陀螺仪的机器人定位方法 | |
CN111113414B (zh) | 一种基于屏幕标识的机器人三维空间尺度提示方法及系统 | |
CN201163363Y (zh) | 数控机床上的通用多轴运动控制系统 | |
CN108466266A (zh) | 机械臂运动控制方法及系统 | |
CN109186605B (zh) | 一种基于uwb室内定位的无人艇船载测速测向方法 | |
CN108646759B (zh) | 基于立体视觉的智能可拆卸移动机器人系统及控制方法 | |
CN111267090A (zh) | 一种双臂机器人主从控制系统及方法 | |
CN106023320A (zh) | 一种嵌入式操作系统三维图形制作软件 | |
CN102117173A (zh) | 一种识别笔画输入的方法和终端 | |
CN111060110A (zh) | 机器人导航方法、机器人导航装置及机器人 | |
CN103552072B (zh) | 一种基于嵌入式控制器的机器人控制方法和装置 | |
CN105269556A (zh) | 机器人的运动前瞻方法 | |
CN202533877U (zh) | 电子手写笔 |
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 |