CN110879571B - 基于圆弧插补获得写字机器人圆弧行走线段的方法 - Google Patents

基于圆弧插补获得写字机器人圆弧行走线段的方法 Download PDF

Info

Publication number
CN110879571B
CN110879571B CN201911288247.7A CN201911288247A CN110879571B CN 110879571 B CN110879571 B CN 110879571B CN 201911288247 A CN201911288247 A CN 201911288247A CN 110879571 B CN110879571 B CN 110879571B
Authority
CN
China
Prior art keywords
line segment
float
arc
circular arc
calculating
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
Application number
CN201911288247.7A
Other languages
English (en)
Other versions
CN110879571A (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.)
Guangzhou College of South China University of Technology
Original Assignee
Guangzhou College of South China University of Technology
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 Guangzhou College of South China University of Technology filed Critical Guangzhou College of South China University of Technology
Priority to CN201911288247.7A priority Critical patent/CN110879571B/zh
Publication of CN110879571A publication Critical patent/CN110879571A/zh
Application granted granted Critical
Publication of CN110879571B publication Critical patent/CN110879571B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/41—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by interpolation, e.g. the computation of intermediate points between programmed end points to define the path to be followed and the rate of travel along that path
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/34—Director, elements to supervisory
    • G05B2219/34083—Interpolation general

Landscapes

  • Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了圆弧插补获得写字机器人圆弧行走线段的方法，具体步骤为，获取圆心坐标、圆心到起始点和终止点的向量坐标，利用余弦公式计算两向量的夹角，通过夹角判断移动方向，计算圆弧分为线段的数目，利用泰勒公式计算每一小线段的sin,cos值，利用圆的极坐标方程计算小线段的起始点和终止点坐标，进行直线插补。利用本方法，对给出的圆弧线段进行分段获得更小的线段，并获得线段的起始坐标和终点坐标，从而便于进行直线插补，以控制写字机器写出更为精确的对应圆弧。

Description

基于圆弧插补获得写字机器人圆弧行走线段的方法
技术领域
本发明涉及基于圆弧插补获得写字机器人圆弧行走线段的方法。
背景技术
在数控系统、机器人控制等领域要求完成空间上的移动。这些移动轨迹需要准确定位，而且必须沿着所希望的路径在一定的精度范围内移动，即要进行精确的连续轨迹控制。通常情况下，仅需要将运动路径上的某些关键点确定下来，然后根据轨迹特征算出这些点之间必须到达的中间位置点，通过插补进行控制，从而实现高效高精的运动控制，运动路径一般由一些基本曲线组成。
现有技术中对于圆弧插补算法适用于不同机器人的算法，而不同机器人的算法又不同，针对用于遥控写字机器人需要一种智能遥控写字机器人的圆弧插补算法来实现用于对智能遥控写字机器人的圆弧算法。
发明内容
本发明的主要目的是为了提供一种基于圆弧插补获得写字机器人圆弧行走线段的方法，利用本方法，对给出的圆弧线段进行分段获得更小的线段，并获得线段的起始坐标和终点坐标，从而便于进行直线插补，以控制写字机器写出更为精确的对应圆弧。
为达到上述目的，基于圆弧插补获得写字机器人圆弧行走线段的方法，包括如下步骤：
步骤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：计算出下一条线段的起始坐标，也就是当前线段的终点坐标，已知当前线段的起始坐标，这样把线段的坐标传递给直线插补函数进行线段插补。
在步骤2中，根据向量夹角余弦公式，推出向量夹角正切公式，套入向量b和向量c的坐标，即可得出tanA；下面公式中的atan2是反正切函数，A=atan2(y,x)，即夹角A=y/x的正切值。
如果圆弧顺时针移动，A应该是负值，如果计算出的A为正值，则在计算出的夹角A基础上减去2*pi；如果圆弧逆时针移动，A应该是正值，如果计算出的A为负值，则在计算出的夹角A基础上加上2*pi，其中，pi为圆周率。进一步的，在步骤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)；
其中， axis_0，圆弧所在平面的第一个轴，是x/y/z中任意一个；
axis_1，圆弧所在平面的第二个轴，是x/y/z中任意一个。进一步的，在步骤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。
本发明的有益技术效果：
本发明基于圆弧插补获得写字机器人圆弧行走线段的方法，通过获取圆心坐标、圆心到起始点和终止点的向量坐标，利用余弦公式计算两向量的夹角，通过夹角判断移动方向，计算圆弧分为线段的数目，利用泰勒公式计算每一小线段的sinT,cosT值，利用圆的极坐标方程计算线段的起始点和终止点坐标，从而实现对圆弧的分段，并实现每一线段的准确直线运动长度和位置，给直线插补提供了准确的插补数据，以控制写字机器写出更为精确的对应圆弧。
附图说明
图1为基于圆弧插补获得写字机器人圆弧行走线段的方法的流程图。
图2为计算线段数目的示意图。
具体实施方式
为使本领域技术人员更加清楚和明确本发明的技术方案，下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
如图1所示，基于圆弧插补获得写字机器人圆弧行走线段的方法，包括如下步骤：
步骤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];
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(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；如图2所示，设定线段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中任意一个。
本发明基于圆弧插补获得写字机器人圆弧行走线段的方法，通过获取圆心坐标、圆心到起始点和终止点的向量坐标，利用余弦公式计算两向量的夹角，通过夹角判断移动方向，计算圆弧分为线段的数目，利用泰勒公式计算每一小线段的sinT,cosT值，利用圆的极坐标方程计算线段的起始点和终止点坐标，从而实现对圆弧的分段，并实现每一线段的准确直线运动长度和位置，给直线插补提供了准确的插补数据，以控制写字机器写出更为精确的对应圆弧。

Claims (3)

1.基于圆弧插补获得写字机器人圆弧行走线段的方法，其特征在于，包括如下步骤：
步骤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)的夹角；
在该步骤中，根据向量夹角余弦公式，推出向量夹角正切公式，套入向量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；设定线段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：计算出下一条线段的起始坐标，也就是当前线段的终点坐标，已知当前线段的起始坐标，这样把线段的坐标传递给直线插补函数进行线段插补。
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];
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)；
其中， axis_0，圆弧所在平面的第一个轴，是x/y/z中任意一个；
axis_1，圆弧所在平面的第二个轴，是x/y/z中任意一个。
3.根据权利要求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。
CN201911288247.7A 2019-12-16 2019-12-16 基于圆弧插补获得写字机器人圆弧行走线段的方法 Active CN110879571B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911288247.7A CN110879571B (zh) 2019-12-16 2019-12-16 基于圆弧插补获得写字机器人圆弧行走线段的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911288247.7A CN110879571B (zh) 2019-12-16 2019-12-16 基于圆弧插补获得写字机器人圆弧行走线段的方法

Publications (2)

Publication Number Publication Date
CN110879571A CN110879571A (zh) 2020-03-13
CN110879571B true CN110879571B (zh) 2021-03-02

Family

ID=69731719

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911288247.7A Active CN110879571B (zh) 2019-12-16 2019-12-16 基于圆弧插补获得写字机器人圆弧行走线段的方法

Country Status (1)

Country Link
CN (1) CN110879571B (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4498061B2 (ja) * 2004-08-18 2010-07-07 株式会社ダイヘン 溶接ロボット制御装置
JP2007316862A (ja) * 2006-05-24 2007-12-06 Yaskawa Electric Corp サーボドライバおよび複数軸のサーボシステム
CN105171743B (zh) * 2015-07-29 2017-05-03 浙江理工大学 码垛机械臂末端的运动轨迹规划方法
CN105676787A (zh) * 2015-12-28 2016-06-15 龙兵 一种椭圆弧插补算法
CN107553484B (zh) * 2016-06-30 2020-07-07 沈阳新松机器人自动化股份有限公司 一种机器人作业运动路径规划方法及系统
CN109176526A (zh) * 2018-10-08 2019-01-11 南京信息工程大学 一种三轴直角坐标机器人空间圆弧插补方法

Also Published As

Publication number Publication date
CN110879571A (zh) 2020-03-13

Similar Documents

Publication Publication Date Title
CN106919171B (zh) 一种机器人室内定位导航系统及方法
CN109313428B (zh) 运动轨迹平滑转接的方法和装置及其相关设备
CN108568817A (zh) 一种基于贝塞尔曲线的Delta机器人轨迹连接控制方法
CN109189060B (zh) 移动机器人的点镇定控制方法及装置
CN111039231B (zh) 智能叉车转弯路径规划方法
Zhou et al. Active target tracking with self-triggered communications in multi-robot teams
CN112270079B (zh) 一种三维自由弯曲主动式弯曲模运动位姿解析方法
CN104661180A (zh) 一种定位频率的调整方法及终端
CN110879571B (zh) 基于圆弧插补获得写字机器人圆弧行走线段的方法
CN107717988A (zh) 一种基于泛克里金的工业机械臂精度标定方法
CN111176482A (zh) 一种圆弧轨迹插补的方法
CN105278462A (zh) 机器人控制系统轨迹加减速插补算法
CN103383552B (zh) 一种任意平面圆弧插补运动控制器及其控制方法
Huang An adjustable look-ahead acceleration/deceleration hybrid interpolation technique with variable maximum feedrate
CN105955274A (zh) 基于分布式圆心和半径估计的多机器人圆形编队控制方法
Xia et al. Localization and circumnavigation of a group of targets by a single agent using bearing measurements
CN103523744B (zh) 高速精密运动平台的两级光栅定位方法
CN115711603A (zh) 一种多智能体系统中无坐标分布式控制算法
CN113359708B (zh) 一种基于相对距离测量的受约束智能体编队控制方法
Chen Terminal sliding mode tracking controller design for Automatic Guided Vehicle
CN111267093A (zh) 一种scara机器人连续轨迹控制实现方法
Wang et al. Ultra-Wideband 3D Indoor Positioning with Wired Clock Synchronization
CN111290376B (zh) 一种水面/水下无人航行器圆形轨迹跟踪方法
CN108732533B (zh) 单辐射源机动飞行目标自定位及自跟踪方法
Yu et al. A prioritized path planning algorithm for MMRS

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