CN111872943B - 一种基于正弦曲线的机器人圆弧轨迹规划方法 - Google Patents

一种基于正弦曲线的机器人圆弧轨迹规划方法 Download PDF

Info

Publication number
CN111872943B
CN111872943B CN202011042293.1A CN202011042293A CN111872943B CN 111872943 B CN111872943 B CN 111872943B CN 202011042293 A CN202011042293 A CN 202011042293A CN 111872943 B CN111872943 B CN 111872943B
Authority
CN
China
Prior art keywords
robot
coordinate system
circular arc
end effector
interpolation
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
CN202011042293.1A
Other languages
English (en)
Other versions
CN111872943A (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.)
Foshan Longshen Robot Co Ltd
Original Assignee
Foshan Longshen Robot Co Ltd
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 Foshan Longshen Robot Co Ltd filed Critical Foshan Longshen Robot Co Ltd
Priority to CN202011042293.1A priority Critical patent/CN111872943B/zh
Publication of CN111872943A publication Critical patent/CN111872943A/zh
Application granted granted Critical
Publication of CN111872943B publication Critical patent/CN111872943B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明提供一种基于正弦曲线的机器人圆弧轨迹规划方法，包括以下步骤：S1：选定三个位置点并获取位置点的位姿；S2：求解得到经过三个位置点的插补圆的所在平面、圆心坐标和半径；S3：建立圆弧坐标系；S4：设定圆弧轨迹的当前插补步数k和插补总步数m，并令k=0；S5：基于正弦曲线计算第k步插补时机器人末端执行器的位姿分速度；S6：计算出机器人关节的角速度；S7：计算出机器人关节的角度；S8：判断k是否等于m；若不等于，则将k+1的值赋予k，并返回步骤S5；若等于，则结束迭代，完成圆弧轨迹的规划。本发明提供一种基于正弦曲线的机器人圆弧轨迹规划方法，解决了目前在规划机器人的运动轨迹时大多都不考虑机器人的关节角度变化的问题。

Description

一种基于正弦曲线的机器人圆弧轨迹规划方法
技术领域
本发明涉及轨迹规划技术领域，更具体的，涉及一种基于正弦曲线的机器人圆弧轨迹规划方法。
背景技术
机器人在运动过程中，需要机器人的各个关节协同运动才能使机器人的末端执行器达成需要的状态，包括空间位置和姿态。但是，机器人的关节个数越多，分解到各个关节的运动计算就越复杂；再加上自定义空间轨迹参数的变化比较复杂，导致每次规划机器人运动的圆弧轨迹都需要重新进行大量的计算。因此，目前在规划机器人的运动轨迹时大多都不考虑机器人的关节角度变化，规划效果不够好。
现有技术中，如2020年05月19日公开的中国专利，一种圆弧轨迹插补的方法，公开号为CN111176482A，通过对设备结构及位置信息采集，建立X\Y轴坐标系，进行插补点的设定，实现圆弧触摸检测的轨迹设定，但没有考虑机器人的关节角度变化，不适用于规划机器人的运动轨迹。
发明内容
本发明为克服目前在规划机器人的运动轨迹时大多都不考虑机器人的关节角度变化的技术缺陷，提供一种基于正弦曲线的机器人圆弧轨迹规划方法。
为解决上述技术问题，本发明的技术方案如下：
一种基于正弦曲线的机器人圆弧轨迹规划方法，包括以下步骤：
S1：选定不在同一直线的三个位置点并获取所述位置点在机器人坐标系下的位姿；
S2：根据所述位置点的位姿求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；
S3：以插补圆的圆心为原点建立圆弧坐标系，并获取圆弧坐标系和机器人坐标系之间的转换关系
Figure 877245DEST_PATH_IMAGE001
；
S4：设定圆弧轨迹的当前插补步数k和插补总步数m，并令k=0；其中，k=0，1，…，m；
S5：基于正弦曲线计算第k步插补时机器人末端执行器在圆弧坐标系中的位姿分速度；
S6：根据转换关系
Figure 450309DEST_PATH_IMAGE001
，将机器人末端执行器在圆弧坐标系中的位姿分速度转换到机器人坐标系下表示，并由位姿分速度计算出机器人关节的角速度；
S7：根据机器人关节的角速度计算出机器人关节的角度，从而得到第k步插补时机器人关节的角度变化；
S8：判断当前插补步数k是否等于插补总步数m；
若不等于，则将k+1的值赋予k，并返回步骤S5；
若等于，则结束迭代，完成圆弧轨迹的规划。
上述方案中，首先由三个不共线的位置点确定插补圆，然后基于正弦曲线求解机器人末端执行器的位姿分速度，再由机器人末端执行器的位姿分速度计算出机器人关节的角速度，进而求解出机器人关节的角度，最后通过迭代循环至当前插补步数k达到插补总步数m，完成圆弧轨迹的规划，实现了根据机器人的关节角度变化来规划机器人的圆弧运动轨迹，提高了圆弧轨迹的规划效果。
优选的，在步骤S1中，三个所述位置点的位姿分别为
Figure 278587DEST_PATH_IMAGE002
，
Figure 75642DEST_PATH_IMAGE003
，
Figure 153319DEST_PATH_IMAGE004
；
其中，位置点
Figure 61233DEST_PATH_IMAGE005
在机器人坐标系下的坐标为
Figure 540755DEST_PATH_IMAGE006
，位置点
Figure 508711DEST_PATH_IMAGE005
在机器人坐标系下的姿态为
Figure 11368DEST_PATH_IMAGE007
；位置点
Figure 926234DEST_PATH_IMAGE008
在机器人坐标系下的坐标为
Figure 729105DEST_PATH_IMAGE009
，位置点
Figure 336804DEST_PATH_IMAGE008
在机器人坐标系下的姿态为
Figure 651723DEST_PATH_IMAGE010
；位置点
Figure 370281DEST_PATH_IMAGE011
在机器人坐标系下的坐标为
Figure 355554DEST_PATH_IMAGE012
，位置点
Figure 806258DEST_PATH_IMAGE011
在机器人坐标系下的姿态为
Figure 876983DEST_PATH_IMAGE013
。
优选的，步骤S2具体为：
根据所述位置点在机器人坐标系下的坐标求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；
设插补圆在机器人坐标系下的圆心坐标为
Figure 664810DEST_PATH_IMAGE014
，半径为
Figure 176694DEST_PATH_IMAGE015
，根据圆心坐标和半径的关系得到方程组：
Figure DEST_PATH_IMAGE016
。
优选的，在步骤S3中，圆弧坐标系的X轴、Y轴、Z轴分别为
Figure DEST_PATH_IMAGE017
、
Figure DEST_PATH_IMAGE018
、
Figure DEST_PATH_IMAGE019
，其中，
Figure 470403DEST_PATH_IMAGE017
为插补圆圆心指向位置点
Figure 762844DEST_PATH_IMAGE005
点的向量，
Figure 354362DEST_PATH_IMAGE019
为过插补圆圆心的法向量，
Figure 517491DEST_PATH_IMAGE018
为
Figure 844085DEST_PATH_IMAGE017
和
Figure 889402DEST_PATH_IMAGE019
根据右手定则的方向向量。
优选的，在步骤S5中，位姿分速度包括移动分速度和姿态角速度。
优选的，基于正弦曲线计算机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度的步骤如下：
S5.1.1：计算机器人末端执行器在
Figure DEST_PATH_IMAGE020
时的移动量
Figure DEST_PATH_IMAGE021
、绕插补圆圆心转动的角度
Figure DEST_PATH_IMAGE022
和第k步的插补移动量
Figure DEST_PATH_IMAGE023
：
Figure DEST_PATH_IMAGE024
其中，插补时间
Figure DEST_PATH_IMAGE025
为每步插补所需的时间，
Figure 832081DEST_PATH_IMAGE015
为插补圆的半径，
Figure DEST_PATH_IMAGE026
为
Figure 787399DEST_PATH_IMAGE020
到
Figure DEST_PATH_IMAGE027
时机器人末端执行器绕回转轴
Figure 344282DEST_PATH_IMAGE019
转动的角度，
Figure 283419DEST_PATH_IMAGE019
为过插补圆圆心的法向量；
S5.1.2：计算
Figure 685582DEST_PATH_IMAGE020
时刻机器人末端执行器分别在圆弧坐标系的三个坐标轴上的位置分量：
Figure DEST_PATH_IMAGE028
；
其中，
Figure DEST_PATH_IMAGE029
分别对应
Figure 554793DEST_PATH_IMAGE020
时机器人末端执行器在圆弧坐标系的X轴、Y轴、Z轴上的位置分量；
S5.1.3：计算
Figure 485840DEST_PATH_IMAGE020
到
Figure 443432DEST_PATH_IMAGE027
时机器人末端执行器绕回转轴
Figure 180444DEST_PATH_IMAGE019
旋转的角速度
Figure DEST_PATH_IMAGE030
：
Figure DEST_PATH_IMAGE031
；
S5.1.4：通过将
Figure 579195DEST_PATH_IMAGE029
进行微分得到第k步插补时机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度：
Figure DEST_PATH_IMAGE032
；
其中，
Figure DEST_PATH_IMAGE033
为第k步插补时机器人末端执行器在圆弧坐标系的X轴上的移动分速度，
Figure DEST_PATH_IMAGE034
为第k步插补时机器人末端执行器在圆弧坐标系的Y轴上的移动分速度，
Figure DEST_PATH_IMAGE035
为第k步插补时机器人末端执行器在圆弧坐标系的Z轴上的移动分速度。
优选的，基于正弦曲线计算机器人末端执行器的姿态角速度的步骤如下：
S5.2.1：设机器人末端执行器从起始位置运动到目标位置的总姿态偏转角度为
Figure DEST_PATH_IMAGE036
，将其姿态偏转的角度按正弦曲线插入，则
Figure 87668DEST_PATH_IMAGE020
时刻的姿态偏转角度
Figure DEST_PATH_IMAGE037
为：
Figure DEST_PATH_IMAGE038
；
其中，插补时间
Figure 195870DEST_PATH_IMAGE025
为每步插补所需的时间，
Figure DEST_PATH_IMAGE039
为运动总时间；
在圆弧坐标系中，
Figure 205415DEST_PATH_IMAGE020
时刻由机器人末端执行器绕回转轴
Figure 255410DEST_PATH_IMAGE019
旋转引起的姿态旋转变化矩阵
Figure DEST_PATH_IMAGE040
为：
Figure DEST_PATH_IMAGE041
；
Figure 528260DEST_PATH_IMAGE020
时刻机器人末端执行器的姿态偏转变化矩阵
Figure DEST_PATH_IMAGE042
为：
Figure DEST_PATH_IMAGE043
；
则
Figure 398127DEST_PATH_IMAGE020
时刻的姿态变化矩阵
Figure DEST_PATH_IMAGE044
为：
Figure DEST_PATH_IMAGE045
；
S5.2.2：计算从
Figure 883466DEST_PATH_IMAGE020
时刻到
Figure 850285DEST_PATH_IMAGE027
时刻的第k步姿态变化矩阵：
Figure DEST_PATH_IMAGE046
；
其中，
Figure DEST_PATH_IMAGE047
为
Figure 228789DEST_PATH_IMAGE027
时刻的姿态变化矩阵；
S5.2.3：将机器人末端执行器从
Figure 913848DEST_PATH_IMAGE020
时刻到
Figure 530775DEST_PATH_IMAGE027
时刻的姿态变化认为是绕某一旋转轴
Figure DEST_PATH_IMAGE048
旋转
Figure DEST_PATH_IMAGE049
得到，通过轴-角的关系变换得到旋转轴
Figure 24204DEST_PATH_IMAGE048
和角度变量
Figure 435594DEST_PATH_IMAGE049
：
Figure DEST_PATH_IMAGE050
；
其中，
Figure DEST_PATH_IMAGE051
为
Figure DEST_PATH_IMAGE052
里第一行第一列的元素，
Figure DEST_PATH_IMAGE053
为
Figure 483315DEST_PATH_IMAGE052
里第一行第二列的元素，
Figure DEST_PATH_IMAGE054
为
Figure 903932DEST_PATH_IMAGE052
里第一行第三列的元素，
Figure DEST_PATH_IMAGE055
为
Figure 254798DEST_PATH_IMAGE052
里第二行第一列的元素，
Figure DEST_PATH_IMAGE056
为
Figure 102668DEST_PATH_IMAGE052
里第二行第二列的元素，
Figure DEST_PATH_IMAGE057
为
Figure 168844DEST_PATH_IMAGE052
里第二行第三列的元素，
Figure DEST_PATH_IMAGE058
为
Figure 861994DEST_PATH_IMAGE052
里第三行第一列的元素，
Figure DEST_PATH_IMAGE059
为
Figure 923491DEST_PATH_IMAGE052
里第三行第二列的元素，
Figure DEST_PATH_IMAGE060
为
Figure 348787DEST_PATH_IMAGE052
里第三行第三列的元素，
Figure DEST_PATH_IMAGE061
为旋转轴
Figure 698997DEST_PATH_IMAGE048
在圆弧坐标系的X轴上的分量，
Figure DEST_PATH_IMAGE062
为旋转轴
Figure 867941DEST_PATH_IMAGE048
在圆弧坐标系的Y轴上的分量，
Figure DEST_PATH_IMAGE063
为旋转轴
Figure 781015DEST_PATH_IMAGE048
在圆弧坐标系的Z轴上的分量；
S5.2.4：由旋转轴
Figure 236267DEST_PATH_IMAGE048
求取
Figure 604931DEST_PATH_IMAGE044
的等效旋转矩阵：
Figure DEST_PATH_IMAGE064
；
将等效旋转矩阵进行微分变换得到：
Figure DEST_PATH_IMAGE065
；
其中，
Figure DEST_PATH_IMAGE066
，
Figure DEST_PATH_IMAGE067
的微分形式，
Figure DEST_PATH_IMAGE068
的微分值；
S5.2.5：由微分定义得到
Figure 718512DEST_PATH_IMAGE020
时刻
Figure 754601DEST_PATH_IMAGE044
的微分
Figure DEST_PATH_IMAGE069
：
Figure DEST_PATH_IMAGE070
；
Figure DEST_PATH_IMAGE071
；
其中，
Figure DEST_PATH_IMAGE072
为时间微分，
Figure DEST_PATH_IMAGE073
为单位矩阵；
S5.2.6：计算机器人末端执行器从
Figure 52858DEST_PATH_IMAGE020
时刻到
Figure 846502DEST_PATH_IMAGE027
时刻的姿态变化的角速度：
Figure DEST_PATH_IMAGE074
；
将
Figure DEST_PATH_IMAGE075
代入
Figure DEST_PATH_IMAGE076
，得到机器人末端执行器的姿态角速度为：
Figure DEST_PATH_IMAGE077
。
优选的，在步骤S6中，
Figure 888407DEST_PATH_IMAGE020
时刻机器人末端执行器在圆弧坐标系下的位姿分速度为：
Figure DEST_PATH_IMAGE078
；
根据圆弧坐标系和机器人坐标系之间的转换关系
Figure 731335DEST_PATH_IMAGE001
，得到
Figure 262810DEST_PATH_IMAGE020
时刻机器人末端执行器在机器人坐标系下的位姿分速度：
Figure DEST_PATH_IMAGE079
；
其中，
Figure DEST_PATH_IMAGE080
为机器人末端执行器在圆弧坐标系的X轴上的姿态角速度分量，
Figure DEST_PATH_IMAGE081
为机器人末端执行器在圆弧坐标系的Y轴上的姿态角速度分量，
Figure DEST_PATH_IMAGE082
为机器人末端执行器在圆弧坐标系的Z轴上的姿态角速度分量。
优选的，在步骤S6中，由位姿分速度
Figure DEST_PATH_IMAGE083
计算出机器人关节的角速度
Figure DEST_PATH_IMAGE084
：
Figure DEST_PATH_IMAGE085
；
其中，
Figure DEST_PATH_IMAGE086
为机器人末端执行器和关节的速度雅可比矩阵，
Figure DEST_PATH_IMAGE087
，
Figure DEST_PATH_IMAGE088
为机器人第一关节的角速度，
Figure DEST_PATH_IMAGE089
为机器人第二关节的角速度，
Figure DEST_PATH_IMAGE090
为机器人第三关节的角速度，
Figure DEST_PATH_IMAGE091
为机器人第四关节的角速度，
Figure DEST_PATH_IMAGE092
为机器人第五关节的角速度，
Figure DEST_PATH_IMAGE093
为机器人第六关节的角速度。
优选的，在步骤S7中，通过对机器人关节的角速度进行积分计算得到机器人关节的角度：
Figure DEST_PATH_IMAGE094
；
其中，
Figure DEST_PATH_IMAGE095
为第k步插补时机器人关节转动的角度，插补时间
Figure 701007DEST_PATH_IMAGE025
为每步插补所需的时间。
与现有技术相比，本发明技术方案的有益效果是：
本发明提供了一种基于正弦曲线的机器人圆弧轨迹规划方法，首先由三个不共线的位置点确定插补圆，然后基于正弦曲线求解机器人末端执行器的位姿分速度，再由机器人末端执行器的位姿分速度计算出机器人关节的角速度，进而求解出机器人关节的角度，最后通过迭代循环至当前插补步数k达到插补总步数m，完成圆弧轨迹的规划，实现了根据机器人的关节角度变化来规划机器人的圆弧运动轨迹，提高了圆弧轨迹的规划效果。
附图说明
图1为本发明的技术方案实施步骤流程图；
图2为本发明中圆弧坐标系的示意图；
图3为本发明中机器人末端执行器在圆弧轨迹中的姿态示意图；
图4为本发明中机器人关节在圆弧轨迹中的角度-时间关系示意图。
具体实施方式
附图仅用于示例性说明，不能理解为对本专利的限制；
为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；
对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
下面结合附图和实施例对本发明的技术方案做进一步的说明。
实施例1
如图1所示，一种基于正弦曲线的机器人圆弧轨迹规划方法，包括以下步骤：
S1：选定不在同一直线的三个位置点并获取所述位置点在机器人坐标系下的位姿；
S2：根据所述位置点的位姿求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；
S3：以插补圆的圆心为原点建立圆弧坐标系，并获取圆弧坐标系和机器人坐标系之间的转换关系
Figure 608920DEST_PATH_IMAGE001
；
S4：设定圆弧轨迹的当前插补步数k和插补总步数m，并令k=0；其中，k=0，1，…，m；
S5：基于正弦曲线计算第k步插补时机器人末端执行器在圆弧坐标系中的位姿分速度；
S6：根据转换关系
Figure 351093DEST_PATH_IMAGE001
，将机器人末端执行器在圆弧坐标系中的位姿分速度转换到机器人坐标系下表示，并由位姿分速度计算出机器人关节的角速度；
S7：根据机器人关节的角速度计算出机器人关节的角度，从而得到第k步插补时机器人关节的角度变化；
S8：判断当前插补步数k是否等于插补总步数m；
若不等于，则将k+1的值赋予k，并返回步骤S5；
若等于，则结束迭代，完成圆弧轨迹的规划。
在具体实施过程中，首先由三个不共线的位置点确定插补圆，然后基于正弦曲线求解机器人末端执行器的位姿分速度，再由机器人末端执行器的位姿分速度计算出机器人关节的角速度，进而求解出机器人关节的角度，最后通过迭代循环至当前插补步数k达到插补总步数m，完成圆弧轨迹的规划，实现了根据机器人的关节角度变化来规划机器人的圆弧运动轨迹，使机器人能够完成一个完整的圆弧轨迹运动，提高了圆弧轨迹的规划效果。
更具体的，在步骤S1中，三个所述位置点的位姿分别为
Figure 319049DEST_PATH_IMAGE002
，
Figure 884022DEST_PATH_IMAGE003
，
Figure 267730DEST_PATH_IMAGE004
；
其中，位置点
Figure 398497DEST_PATH_IMAGE005
在机器人坐标系下的坐标为
Figure 6196DEST_PATH_IMAGE006
，位置点
Figure 324045DEST_PATH_IMAGE005
在机器人坐标系下的姿态为
Figure 573761DEST_PATH_IMAGE007
；位置点
Figure 559034DEST_PATH_IMAGE008
在机器人坐标系下的坐标为
Figure 540897DEST_PATH_IMAGE009
，位置点
Figure 814883DEST_PATH_IMAGE008
在机器人坐标系下的姿态为
Figure 868290DEST_PATH_IMAGE010
；位置点
Figure 442491DEST_PATH_IMAGE011
在机器人坐标系下的坐标为
Figure 923150DEST_PATH_IMAGE012
，位置点
Figure 950012DEST_PATH_IMAGE011
在机器人坐标系下的姿态为
Figure 541531DEST_PATH_IMAGE013
。
在具体实施过程中，机器人坐标系由机器人建立，能够从机器人中直接获得。
在具体实施过程中，位姿包括位置和姿态，位置为位置点在机器人坐标系下的坐标。
更具体的，步骤S2具体为：
根据所述位置点在机器人坐标系下的坐标求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；
设插补圆在机器人坐标系下的圆心坐标为
Figure 173500DEST_PATH_IMAGE014
，半径为
Figure 825061DEST_PATH_IMAGE015
，根据圆心坐标和半径的关系得到方程组：
Figure DEST_PATH_IMAGE096
。
更具体的，如图2所示，在步骤S3中，圆弧坐标系的X轴、Y轴、Z轴分别为
Figure 73640DEST_PATH_IMAGE017
、
Figure 203270DEST_PATH_IMAGE018
、
Figure 486484DEST_PATH_IMAGE019
，其中，
Figure 308946DEST_PATH_IMAGE017
为插补圆圆心指向位置点
Figure 251013DEST_PATH_IMAGE005
点的向量，
Figure 449914DEST_PATH_IMAGE019
为过插补圆圆心的法向量，
Figure 853213DEST_PATH_IMAGE018
为
Figure 846577DEST_PATH_IMAGE017
和
Figure 69748DEST_PATH_IMAGE019
根据右手定则的方向向量。
更具体的，在步骤S5中，位姿分速度包括移动分速度和姿态角速度。
更具体的，基于正弦曲线计算机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度的步骤如下：
S5.1.1：计算机器人末端执行器在
Figure 744443DEST_PATH_IMAGE020
时的移动量
Figure 267828DEST_PATH_IMAGE021
、绕插补圆圆心转动的角度
Figure 900935DEST_PATH_IMAGE022
和第k步的插补移动量
Figure 142560DEST_PATH_IMAGE023
：
Figure 417684DEST_PATH_IMAGE024
其中，插补时间
Figure 795575DEST_PATH_IMAGE025
为每步插补所需的时间，
Figure 865162DEST_PATH_IMAGE015
为插补圆的半径，
Figure 328505DEST_PATH_IMAGE026
为
Figure 345002DEST_PATH_IMAGE020
到
Figure 311821DEST_PATH_IMAGE027
时机器人末端执行器绕回转轴
Figure 83468DEST_PATH_IMAGE019
转动的角度，
Figure 502948DEST_PATH_IMAGE019
为过插补圆圆心的法向量；
S5.1.2：计算
Figure 385454DEST_PATH_IMAGE020
时刻机器人末端执行器分别在圆弧坐标系的三个坐标轴上的位置分量：
Figure 410042DEST_PATH_IMAGE028
；
其中，
Figure 352590DEST_PATH_IMAGE029
分别对应
Figure 259366DEST_PATH_IMAGE020
时机器人末端执行器在圆弧坐标系的X轴、Y轴、Z轴上的位置分量；
S5.1.3：计算
Figure 945562DEST_PATH_IMAGE020
到
Figure 884044DEST_PATH_IMAGE027
时机器人末端执行器绕回转轴
Figure 997493DEST_PATH_IMAGE019
旋转的角速度
Figure 391565DEST_PATH_IMAGE030
：
Figure 615873DEST_PATH_IMAGE031
；
S5.1.4：通过将
Figure 615053DEST_PATH_IMAGE029
进行微分得到第k步插补时机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度：
Figure 102666DEST_PATH_IMAGE032
；
其中，
Figure 515193DEST_PATH_IMAGE033
为第k步插补时机器人末端执行器在圆弧坐标系的X轴上的移动分速度，
Figure 277613DEST_PATH_IMAGE034
为第k步插补时机器人末端执行器在圆弧坐标系的Y轴上的移动分速度，
Figure 459196DEST_PATH_IMAGE035
为第k步插补时机器人末端执行器在圆弧坐标系的Z轴上的移动分速度。
更具体的，基于正弦曲线计算机器人末端执行器的姿态角速度的步骤如下：
S5.2.1：设机器人末端执行器从起始位置运动到目标位置的总姿态偏转角度为
Figure 586552DEST_PATH_IMAGE036
，将其姿态偏转的角度按正弦曲线插入，则
Figure 220795DEST_PATH_IMAGE020
时刻的姿态偏转角度
Figure 255747DEST_PATH_IMAGE037
为：
Figure 557416DEST_PATH_IMAGE038
；
其中，插补时间
Figure 917990DEST_PATH_IMAGE025
为每步插补所需的时间，
Figure 508371DEST_PATH_IMAGE039
为运动总时间；
在圆弧坐标系中，
Figure 878173DEST_PATH_IMAGE020
时刻由机器人末端执行器绕回转轴
Figure 706451DEST_PATH_IMAGE019
旋转引起的姿态旋转变化矩阵
Figure 503506DEST_PATH_IMAGE040
为：
Figure 581183DEST_PATH_IMAGE041
；
Figure 489097DEST_PATH_IMAGE020
时刻机器人末端执行器的姿态偏转变化矩阵
Figure 234199DEST_PATH_IMAGE042
为：
Figure 202155DEST_PATH_IMAGE043
；
则
Figure 767128DEST_PATH_IMAGE020
时刻的姿态变化矩阵
Figure 478732DEST_PATH_IMAGE044
为：
Figure 7235DEST_PATH_IMAGE045
；
S5.2.2：计算从
Figure 146092DEST_PATH_IMAGE020
时刻到
Figure 463941DEST_PATH_IMAGE027
时刻的第k步姿态变化矩阵：
Figure 979236DEST_PATH_IMAGE046
；
其中，
Figure 167772DEST_PATH_IMAGE047
为
Figure 477531DEST_PATH_IMAGE027
时刻的姿态变化矩阵；
S5.2.3：将机器人末端执行器从
Figure 220359DEST_PATH_IMAGE020
时刻到
Figure 742607DEST_PATH_IMAGE027
时刻的姿态变化认为是绕某一旋转轴
Figure 582387DEST_PATH_IMAGE048
旋转
Figure 797468DEST_PATH_IMAGE049
得到，通过轴-角的关系变换得到旋转轴
Figure 355488DEST_PATH_IMAGE048
和角度变量
Figure 415848DEST_PATH_IMAGE049
：
Figure 110134DEST_PATH_IMAGE050
；
其中，
Figure 699379DEST_PATH_IMAGE051
为
Figure 479116DEST_PATH_IMAGE052
里第一行第一列的元素，
Figure 608746DEST_PATH_IMAGE053
为
Figure 891960DEST_PATH_IMAGE052
里第一行第二列的元素，
Figure 714422DEST_PATH_IMAGE054
为
Figure 653559DEST_PATH_IMAGE052
里第一行第三列的元素，
Figure 852459DEST_PATH_IMAGE055
为
Figure 990180DEST_PATH_IMAGE052
里第二行第一列的元素，
Figure 983544DEST_PATH_IMAGE056
为
Figure 469364DEST_PATH_IMAGE052
里第二行第二列的元素，
Figure 675217DEST_PATH_IMAGE057
为
Figure 198603DEST_PATH_IMAGE052
里第二行第三列的元素，
Figure 300551DEST_PATH_IMAGE058
为
Figure 542176DEST_PATH_IMAGE052
里第三行第一列的元素，
Figure 286141DEST_PATH_IMAGE059
为
Figure 929612DEST_PATH_IMAGE052
里第三行第二列的元素，
Figure 999200DEST_PATH_IMAGE060
为
Figure 728121DEST_PATH_IMAGE052
里第三行第三列的元素，
Figure 479039DEST_PATH_IMAGE061
为旋转轴
Figure 977017DEST_PATH_IMAGE048
在圆弧坐标系的X轴上的分量，
Figure 217505DEST_PATH_IMAGE062
为旋转轴
Figure 902565DEST_PATH_IMAGE048
在圆弧坐标系的Y轴上的分量，
Figure 785070DEST_PATH_IMAGE063
为旋转轴
Figure 606395DEST_PATH_IMAGE048
在圆弧坐标系的Z轴上的分量；
S5.2.4：由旋转轴
Figure 548944DEST_PATH_IMAGE048
求取
Figure 658982DEST_PATH_IMAGE044
的等效旋转矩阵：
Figure 79599DEST_PATH_IMAGE064
；
将等效旋转矩阵进行微分变换得到：
Figure 21010DEST_PATH_IMAGE065
；
其中，
Figure 134460DEST_PATH_IMAGE066
，
Figure 794111DEST_PATH_IMAGE067
的微分形式，
Figure 18419DEST_PATH_IMAGE068
的微分值；
S5.2.5：由微分定义得到
Figure 548758DEST_PATH_IMAGE020
时刻
Figure 833109DEST_PATH_IMAGE044
的微分
Figure 920669DEST_PATH_IMAGE069
：
Figure 948668DEST_PATH_IMAGE070
；
Figure DEST_PATH_IMAGE097
；
其中，
Figure 864671DEST_PATH_IMAGE072
为时间微分，
Figure 523186DEST_PATH_IMAGE073
为单位矩阵；
S5.2.6：计算机器人末端执行器从
Figure 423009DEST_PATH_IMAGE020
时刻到
Figure 661223DEST_PATH_IMAGE027
时刻的姿态变化的角速度：
Figure 431733DEST_PATH_IMAGE074
；
将
Figure 792307DEST_PATH_IMAGE075
代入
Figure 913847DEST_PATH_IMAGE076
，得到机器人末端执行器的姿态角速度为：
Figure 283648DEST_PATH_IMAGE077
。
在具体实施过程中，机器人末端执行器在圆弧运动过程中，除了会围绕回转轴
Figure DEST_PATH_IMAGE099
旋转引起姿态旋转之外，还会产生姿态偏转，如图3所示，
Figure DEST_PATH_IMAGE101
分别为机器人末端执行器无偏转时的旋转矢量，
Figure DEST_PATH_IMAGE103
分别为机器人末端执行器有偏转时的旋转矢量，两个状态之间的旋转矢量偏转角度为
Figure DEST_PATH_IMAGE105
。
更具体的，在步骤S6中，
Figure 190556DEST_PATH_IMAGE020
时刻机器人末端执行器在圆弧坐标系下的位姿分速度为：
Figure 987610DEST_PATH_IMAGE078
；
根据圆弧坐标系和机器人坐标系之间的转换关系
Figure 65288DEST_PATH_IMAGE001
，得到
Figure 238780DEST_PATH_IMAGE020
时刻机器人末端执行器在机器人坐标系下的位姿分速度：
Figure 980952DEST_PATH_IMAGE079
；
其中，
Figure 417750DEST_PATH_IMAGE080
为机器人末端执行器在圆弧坐标系的X轴上的姿态角速度分量，
Figure 513882DEST_PATH_IMAGE081
为机器人末端执行器在圆弧坐标系的Y轴上的姿态角速度分量，
Figure 897590DEST_PATH_IMAGE082
为机器人末端执行器在圆弧坐标系的Z轴上的姿态角速度分量。
更具体的，在步骤S6中，由位姿分速度
Figure 762778DEST_PATH_IMAGE083
计算出机器人关节的角速度
Figure 370477DEST_PATH_IMAGE084
：
Figure 953905DEST_PATH_IMAGE085
；
其中，
Figure 938041DEST_PATH_IMAGE086
为机器人末端执行器和关节的速度雅可比矩阵，
Figure 923315DEST_PATH_IMAGE087
，
Figure 905177DEST_PATH_IMAGE088
为机器人第一关节的角速度，
Figure 975901DEST_PATH_IMAGE089
为机器人第二关节的角速度，
Figure 498150DEST_PATH_IMAGE090
为机器人第三关节的角速度，
Figure 337930DEST_PATH_IMAGE091
为机器人第四关节的角速度，
Figure 553010DEST_PATH_IMAGE092
为机器人第五关节的角速度，
Figure 845451DEST_PATH_IMAGE093
为机器人第六关节的角速度。如图4所示，关节1、关节2、关节3、关节4、关节5、关节6分别对应第一关节、第二关节、第三关节、第四关节、第五关节、第六关节，图中的曲线分别为机器人各个关节的角度随当前插补步数k（即插补时间）的变化情况，通过将机器人的各个关节的角度对时间求导，得到相应关节的角速度，即
Figure 171391DEST_PATH_IMAGE088
、
Figure 865677DEST_PATH_IMAGE089
、
Figure 189342DEST_PATH_IMAGE090
、
Figure 703500DEST_PATH_IMAGE091
、
Figure 98709DEST_PATH_IMAGE092
、
Figure 381923DEST_PATH_IMAGE093
。
在具体实施过程中，机器人末端执行器和关节的速度雅可比矩阵根据实际情况设置。
更具体的，在步骤S7中，通过对机器人关节的角速度进行积分计算得到机器人关节的角度：
Figure 469965DEST_PATH_IMAGE094
；
其中，
Figure 205840DEST_PATH_IMAGE095
为第k步插补时机器人关节转动的角度，插补时间
Figure 404740DEST_PATH_IMAGE025
为每步插补所需的时间。
在具体实施过程中，由于机器人在进行圆弧轨迹运动的时候，需要机器人的各个关节协同运动才能使机器人末端执行器达成需要的状态，因此通过对从
Figure DEST_PATH_IMAGE107
时刻到
Figure DEST_PATH_IMAGE109
时刻的角速度积分，得到第k步插补时机器人关节需要转动的角度，从而实现机器人圆弧轨迹规划，使机器人进行圆弧轨迹运动。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，包括以下步骤：
S1：选定不在同一直线的三个位置点并获取所述位置点在机器人坐标系下的位姿；
S2：根据所述位置点的位姿求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；
S3：以插补圆的圆心为原点建立圆弧坐标系，并获取圆弧坐标系和机器人坐标系之间的转换关系
Figure 17215DEST_PATH_IMAGE001
；
S4：设定圆弧轨迹的当前插补步数k和插补总步数m，并令k=0；其中，k=0，1，…，m；
S5：基于正弦曲线计算第k步插补时机器人末端执行器在圆弧坐标系中的位姿分速度；
S6：根据转换关系
Figure 967853DEST_PATH_IMAGE001
，将机器人末端执行器在圆弧坐标系中的位姿分速度转换到机器人坐标系下表示，并由位姿分速度计算出机器人关节的角速度；
S7：根据机器人关节的角速度计算出机器人关节的角度，从而得到第k步插补时机器人关节的角度变化；
S8：判断当前插补步数k是否等于插补总步数m；
若不等于，则将k+1的值赋予k，并返回步骤S5；
若等于，则结束迭代，完成圆弧轨迹的规划。
2.根据权利要求1所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，在步骤S1中，三个所述位置点的位姿分别为
Figure 319200DEST_PATH_IMAGE002
，
Figure 343788DEST_PATH_IMAGE003
，
Figure 286336DEST_PATH_IMAGE004
；
其中，位置点
Figure 193112DEST_PATH_IMAGE005
在机器人坐标系下的坐标为
Figure 879308DEST_PATH_IMAGE006
，位置点
Figure 820720DEST_PATH_IMAGE005
在机器人坐标系下的姿态为
Figure 403011DEST_PATH_IMAGE007
；位置点
Figure 328241DEST_PATH_IMAGE008
在机器人坐标系下的坐标为
Figure 224653DEST_PATH_IMAGE009
，位置点
Figure 551729DEST_PATH_IMAGE008
在机器人坐标系下的姿态为
Figure 304922DEST_PATH_IMAGE010
；位置点
Figure 717449DEST_PATH_IMAGE011
在机器人坐标系下的坐标为
Figure 214289DEST_PATH_IMAGE012
，位置点
Figure 395872DEST_PATH_IMAGE011
在机器人坐标系下的姿态为
Figure 523228DEST_PATH_IMAGE013
。
3.根据权利要求2所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，步骤S2具体为：
根据所述位置点在机器人坐标系下的坐标求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；
设插补圆在机器人坐标系下的圆心坐标为
Figure 626313DEST_PATH_IMAGE014
，半径为
Figure 192423DEST_PATH_IMAGE015
，根据圆心坐标和半径的关系得到方程组：
Figure 225583DEST_PATH_IMAGE017
。
4.根据权利要求1所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，在步骤S3中，圆弧坐标系的X轴、Y轴、Z轴分别为
Figure 851736DEST_PATH_IMAGE018
、
Figure 442118DEST_PATH_IMAGE019
、
Figure 484023DEST_PATH_IMAGE020
，其中，
Figure 640198DEST_PATH_IMAGE018
为插补圆圆心指向位置点
Figure 906094DEST_PATH_IMAGE005
点的向量，
Figure 249351DEST_PATH_IMAGE020
为过插补圆圆心的法向量，
Figure 829368DEST_PATH_IMAGE019
为
Figure 308891DEST_PATH_IMAGE018
和
Figure 276847DEST_PATH_IMAGE020
根据右手定则的方向向量。
5.根据权利要求1所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，在步骤S5中，位姿分速度包括移动分速度和姿态角速度。
6.根据权利要求5所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，基于正弦曲线计算机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度的步骤如下：
S5.1.1：计算机器人末端执行器在
Figure 107399DEST_PATH_IMAGE021
时的移动量
Figure 287845DEST_PATH_IMAGE022
、绕插补圆圆心转动的角度
Figure 825137DEST_PATH_IMAGE023
和第k步的插补移动量
Figure 432835DEST_PATH_IMAGE024
：
Figure 16264DEST_PATH_IMAGE025
其中，插补时间
Figure 265979DEST_PATH_IMAGE026
为每步插补所需的时间，
Figure 454515DEST_PATH_IMAGE015
为插补圆的半径，
Figure 439307DEST_PATH_IMAGE027
为
Figure 244452DEST_PATH_IMAGE021
到
Figure 297859DEST_PATH_IMAGE028
时机器人末端执行器绕回转轴
Figure 606481DEST_PATH_IMAGE020
转动的角度，
Figure 759244DEST_PATH_IMAGE020
为过插补圆圆心的法向量；
S5.1.2：计算
Figure 317265DEST_PATH_IMAGE021
时刻机器人末端执行器分别在圆弧坐标系的三个坐标轴上的位置分量：
Figure 377625DEST_PATH_IMAGE029
；
其中，
Figure 71911DEST_PATH_IMAGE030
分别对应
Figure 723472DEST_PATH_IMAGE021
时机器人末端执行器在圆弧坐标系的X轴、Y轴、Z轴上的位置分量；
S5.1.3：计算
Figure 503209DEST_PATH_IMAGE021
到
Figure 367260DEST_PATH_IMAGE028
时机器人末端执行器绕回转轴
Figure 181632DEST_PATH_IMAGE020
旋转的角速度
Figure 676199DEST_PATH_IMAGE031
：
Figure 943232DEST_PATH_IMAGE032
；
S5.1.4：通过将
Figure 876553DEST_PATH_IMAGE030
进行微分得到第k步插补时机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度：
Figure 14273DEST_PATH_IMAGE033
；
其中，
Figure 7637DEST_PATH_IMAGE034
为第k步插补时机器人末端执行器在圆弧坐标系的X轴上的移动分速度，
Figure 434070DEST_PATH_IMAGE035
为第k步插补时机器人末端执行器在圆弧坐标系的Y轴上的移动分速度，
Figure 171082DEST_PATH_IMAGE036
为第k步插补时机器人末端执行器在圆弧坐标系的Z轴上的移动分速度。
7.根据权利要求6所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，基于正弦曲线计算机器人末端执行器的姿态角速度的步骤如下：
S5.2.1：设机器人末端执行器从起始位置运动到目标位置的总姿态偏转角度为
Figure 163309DEST_PATH_IMAGE037
，将其姿态偏转的角度按正弦曲线插入，则
Figure 324644DEST_PATH_IMAGE021
时刻的姿态偏转角度
Figure 566270DEST_PATH_IMAGE038
为：
Figure 310235DEST_PATH_IMAGE039
；
其中，插补时间
Figure 953706DEST_PATH_IMAGE026
为每步插补所需的时间，
Figure 960976DEST_PATH_IMAGE040
为运动总时间；
在圆弧坐标系中，
Figure 689898DEST_PATH_IMAGE021
时刻由机器人末端执行器绕回转轴
Figure 237554DEST_PATH_IMAGE020
旋转引起的姿态旋转变化矩阵
Figure 735531DEST_PATH_IMAGE041
为：
Figure 241599DEST_PATH_IMAGE042
；
Figure 192237DEST_PATH_IMAGE021
时刻机器人末端执行器的姿态偏转变化矩阵
Figure 746847DEST_PATH_IMAGE043
为：
Figure 568172DEST_PATH_IMAGE044
；
则
Figure 510720DEST_PATH_IMAGE021
时刻的姿态变化矩阵
Figure 683076DEST_PATH_IMAGE045
为：
Figure 103693DEST_PATH_IMAGE046
；
S5.2.2：计算从
Figure 45104DEST_PATH_IMAGE021
时刻到
Figure 158553DEST_PATH_IMAGE028
时刻的第k步姿态变化矩阵：
Figure 755888DEST_PATH_IMAGE047
；
其中，
Figure 980196DEST_PATH_IMAGE048
为
Figure 510534DEST_PATH_IMAGE028
时刻的姿态变化矩阵；
S5.2.3：将机器人末端执行器从
Figure 529306DEST_PATH_IMAGE021
时刻到
Figure 941833DEST_PATH_IMAGE028
时刻的姿态变化认为是绕某一旋转轴
Figure 438673DEST_PATH_IMAGE049
旋转
Figure 620256DEST_PATH_IMAGE050
得到，通过轴-角的关系变换得到旋转轴
Figure 473244DEST_PATH_IMAGE049
和角度变量
Figure 373066DEST_PATH_IMAGE050
：
Figure 673598DEST_PATH_IMAGE051
；
其中，
Figure 975266DEST_PATH_IMAGE052
为
Figure 804682DEST_PATH_IMAGE053
里第一行第一列的元素，
Figure 191801DEST_PATH_IMAGE054
为
Figure 233706DEST_PATH_IMAGE053
里第一行第二列的元素，
Figure 593143DEST_PATH_IMAGE055
为
Figure 390198DEST_PATH_IMAGE053
里第一行第三列的元素，
Figure 733455DEST_PATH_IMAGE056
为
Figure 906947DEST_PATH_IMAGE053
里第二行第一列的元素，
Figure 386470DEST_PATH_IMAGE057
为
Figure 354426DEST_PATH_IMAGE053
里第二行第二列的元素，
Figure 122662DEST_PATH_IMAGE058
为
Figure 834266DEST_PATH_IMAGE053
里第二行第三列的元素，
Figure 433875DEST_PATH_IMAGE059
为
Figure 572732DEST_PATH_IMAGE053
里第三行第一列的元素，
Figure 625001DEST_PATH_IMAGE060
为
Figure 546821DEST_PATH_IMAGE053
里第三行第二列的元素，
Figure 532095DEST_PATH_IMAGE061
为
Figure 310695DEST_PATH_IMAGE053
里第三行第三列的元素，
Figure 381419DEST_PATH_IMAGE062
为旋转轴
Figure 166317DEST_PATH_IMAGE049
在圆弧坐标系的X轴上的分量，
Figure 6097DEST_PATH_IMAGE063
为旋转轴
Figure 955598DEST_PATH_IMAGE049
在圆弧坐标系的Y轴上的分量，
Figure 920143DEST_PATH_IMAGE064
为旋转轴
Figure 777241DEST_PATH_IMAGE049
在圆弧坐标系的Z轴上的分量；
S5.2.4：由旋转轴
Figure 18997DEST_PATH_IMAGE049
求取
Figure 670559DEST_PATH_IMAGE045
的等效旋转矩阵：
Figure 387979DEST_PATH_IMAGE065
；
将等效旋转矩阵进行微分变换得到：
Figure 783188DEST_PATH_IMAGE066
；
其中，
Figure 800823DEST_PATH_IMAGE067
，
Figure 888864DEST_PATH_IMAGE068
的微分形式，
Figure 890318DEST_PATH_IMAGE069
的微分值；
S5.2.5：由微分定义得到
Figure 558060DEST_PATH_IMAGE021
时刻
Figure 961360DEST_PATH_IMAGE045
的微分
Figure 895336DEST_PATH_IMAGE070
：
Figure 649666DEST_PATH_IMAGE071
；
Figure 121098DEST_PATH_IMAGE073
；
其中，
Figure 644484DEST_PATH_IMAGE074
为时间微分，
Figure 543169DEST_PATH_IMAGE075
为单位矩阵；
S5.2.6：计算机器人末端执行器从
Figure 191320DEST_PATH_IMAGE021
时刻到
Figure 200864DEST_PATH_IMAGE028
时刻的姿态变化的角速度：
Figure 578756DEST_PATH_IMAGE076
；
将
Figure 913922DEST_PATH_IMAGE077
代入
Figure 111685DEST_PATH_IMAGE078
，得到机器人末端执行器的姿态角速度为：
Figure 862603DEST_PATH_IMAGE079
。
8.根据权利要求7所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，在步骤S6中，
Figure 360581DEST_PATH_IMAGE021
时刻机器人末端执行器在圆弧坐标系下的位姿分速度为：
Figure 601069DEST_PATH_IMAGE080
；
根据圆弧坐标系和机器人坐标系之间的转换关系
Figure 817287DEST_PATH_IMAGE001
，得到
Figure 168634DEST_PATH_IMAGE021
时刻机器人末端执行器在机器人坐标系下的位姿分速度：
Figure 521118DEST_PATH_IMAGE081
；
其中，
Figure 135770DEST_PATH_IMAGE082
为机器人末端执行器在圆弧坐标系的X轴上的姿态角速度分量，
Figure 573705DEST_PATH_IMAGE083
为机器人末端执行器在圆弧坐标系的Y轴上的姿态角速度分量，
Figure 463163DEST_PATH_IMAGE084
为机器人末端执行器在圆弧坐标系的Z轴上的姿态角速度分量。
9.根据权利要求8所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，在步骤S6中，由位姿分速度
Figure 935733DEST_PATH_IMAGE085
计算出机器人关节的角速度
Figure 515094DEST_PATH_IMAGE086
：
Figure 440325DEST_PATH_IMAGE087
；
其中，
Figure 133474DEST_PATH_IMAGE088
为机器人末端执行器和关节的速度雅可比矩阵，
Figure 460550DEST_PATH_IMAGE089
，
Figure 417005DEST_PATH_IMAGE090
为机器人第一关节的角速度，
Figure 829532DEST_PATH_IMAGE091
为机器人第二关节的角速度，
Figure 60793DEST_PATH_IMAGE092
为机器人第三关节的角速度，
Figure 242376DEST_PATH_IMAGE093
为机器人第四关节的角速度，
Figure 432049DEST_PATH_IMAGE094
为机器人第五关节的角速度，
Figure 331872DEST_PATH_IMAGE095
为机器人第六关节的角速度。
10.根据权利要求9所述的一种基于正弦曲线的机器人圆弧轨迹规划方法，其特征在于，在步骤S7中，通过对机器人关节的角速度进行积分计算得到机器人关节的角度：
Figure 570086DEST_PATH_IMAGE096
；
其中，
Figure 606175DEST_PATH_IMAGE097
为第k步插补时机器人关节转动的角度，插补时间
Figure 701170DEST_PATH_IMAGE026
为每步插补所需的时间。
CN202011042293.1A 2020-09-28 2020-09-28 一种基于正弦曲线的机器人圆弧轨迹规划方法 Active CN111872943B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011042293.1A CN111872943B (zh) 2020-09-28 2020-09-28 一种基于正弦曲线的机器人圆弧轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011042293.1A CN111872943B (zh) 2020-09-28 2020-09-28 一种基于正弦曲线的机器人圆弧轨迹规划方法

Publications (2)

Publication Number Publication Date
CN111872943A CN111872943A (zh) 2020-11-03
CN111872943B true CN111872943B (zh) 2021-02-19

Family

ID=73199155

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011042293.1A Active CN111872943B (zh) 2020-09-28 2020-09-28 一种基于正弦曲线的机器人圆弧轨迹规划方法

Country Status (1)

Country Link
CN (1) CN111872943B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS61217803A (ja) * 1985-03-23 1986-09-27 Kawasaki Heavy Ind Ltd ロボツトの円弧補間方法
EP0380678A1 (en) * 1988-03-10 1990-08-08 Fanuc Ltd. Method of controlling tool attitude of a robot
CN104062976A (zh) * 2014-06-10 2014-09-24 北京控制工程研究所 一种基于角加速度导数为正弦曲线的飞行器姿态快速机动方法
CN105183009A (zh) * 2015-10-15 2015-12-23 哈尔滨工程大学 一种冗余机械臂轨迹控制方法
CN106671079A (zh) * 2015-11-06 2017-05-17 中国科学院沈阳计算技术研究所有限公司 一种实现变位机协同的焊接机器人运动控制方法
CN109343345A (zh) * 2018-09-28 2019-02-15 江南大学 基于qpso算法的机械臂多项式插值轨迹规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS61217803A (ja) * 1985-03-23 1986-09-27 Kawasaki Heavy Ind Ltd ロボツトの円弧補間方法
EP0380678A1 (en) * 1988-03-10 1990-08-08 Fanuc Ltd. Method of controlling tool attitude of a robot
CN104062976A (zh) * 2014-06-10 2014-09-24 北京控制工程研究所 一种基于角加速度导数为正弦曲线的飞行器姿态快速机动方法
CN105183009A (zh) * 2015-10-15 2015-12-23 哈尔滨工程大学 一种冗余机械臂轨迹控制方法
CN106671079A (zh) * 2015-11-06 2017-05-17 中国科学院沈阳计算技术研究所有限公司 一种实现变位机协同的焊接机器人运动控制方法
CN109343345A (zh) * 2018-09-28 2019-02-15 江南大学 基于qpso算法的机械臂多项式插值轨迹规划方法

Also Published As

Publication number Publication date
CN111872943A (zh) 2020-11-03

Similar Documents

Publication Publication Date Title
CN107490965B (zh) 一种空间自由漂浮机械臂的多约束轨迹规划方法
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
Li et al. Visual servo regulation of wheeled mobile robots with simultaneous depth identification
JP3207728B2 (ja) 冗長マニピュレータの制御方法
CN106774309B (zh) 一种移动机器人同时视觉伺服与自适应深度辨识方法
CN108241339A (zh) 仿人机械臂的运动求解和构型控制方法
CN105014677B (zh) 基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法
CN107363813A (zh) 一种基于可穿戴设备的桌面工业机器人示教系统和方法
CN107263466B (zh) 空间机器人基于二次规划问题的基座无扰控制方法
CN111914411B (zh) 一种全姿态四轴转台框架角指令解算方法
CN113601512B (zh) 一种机械臂奇异点的通用规避方法与系统
CN107457783B (zh) 基于pd控制器的六自由度机械臂自适应智能检测方法
Gong et al. An uncalibrated visual servo method based on projective homography
CN107791248B (zh) 基于不满足Pieper准则的六自由度串联机器人的控制方法
CN110722533B (zh) 轮式移动机器人外参数无标定视觉伺服跟踪
Li et al. Visual servoing of wheeled mobile robots without desired images
CN107253191B (zh) 一种双机械臂系统及其协调控制方法
Lü et al. The seam position detection and tracking for the mobile welding robot
CN104867172B (zh) 一种实现刚体运动的方法
CN114721275B (zh) 一种基于预设性能的视觉伺服机器人自适应跟踪控制方法
CN109623812B (zh) 考虑航天器本体姿态运动的机械臂轨迹规划方法
CN106020494A (zh) 基于移动跟踪的三维手势识别方法
CN114571452A (zh) 工业机器人轨迹规划方法、电子设备及可读存储介质
Milenkovic Continuous path control for optimal wrist singularity avoidance in a serial robot
CN115122325A (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