CN113696187B - 一种适用于双机器人系统的防碰撞方法 - Google Patents

一种适用于双机器人系统的防碰撞方法 Download PDF

Info

Publication number
CN113696187B
CN113696187B CN202111233408.XA CN202111233408A CN113696187B CN 113696187 B CN113696187 B CN 113696187B CN 202111233408 A CN202111233408 A CN 202111233408A CN 113696187 B CN113696187 B CN 113696187B
Authority
CN
China
Prior art keywords
industrial robot
connecting rod
coordinate system
rotating shaft
axis
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
CN202111233408.XA
Other languages
English (en)
Other versions
CN113696187A (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.)
Chengdu Aircraft Industrial Group Co Ltd
Original Assignee
Chengdu Aircraft Industrial Group 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 Chengdu Aircraft Industrial Group Co Ltd filed Critical Chengdu Aircraft Industrial Group Co Ltd
Priority to CN202111233408.XA priority Critical patent/CN113696187B/zh
Publication of CN113696187A publication Critical patent/CN113696187A/zh
Application granted granted Critical
Publication of CN113696187B publication Critical patent/CN113696187B/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/1679—Programme controls characterised by the tasks executed
    • B25J9/1682—Dual arm manipulator; Coordination of several manipulators
    • 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
    • B25J9/1666—Avoiding collision or forbidden zones

Landscapes

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

Abstract

本发明公开了一种适用于双机器人系统的防碰撞方法，构建机器人各连杆在同一坐标系下的连杆模型，通过控制系统实时采集工业机器人各关节角度，通过连杆模型实时计算出工业机器人各连杆在空间内的位置，实时检测两台工业机器人连杆之间的最小距离，当最小距离小于或等于设定的阈值时，则控制系统发出碰撞警示信号，并控制工业机器人停止运动。本发明构建了关于工业机器人的数学模型，通过控制系统实时采集机器人各关节角度，实时计算出工业机器人各连杆在空间内的位置，进而实现实时检测两台工业机器人连杆之间的最小距离，然后将该最小距离与设定阈值进行比较，实现实时的对工业机器人进行防碰撞控制，具有较好的实用性。

Description

一种适用于双机器人系统的防碰撞方法
技术领域
本发明属于机器人自动控制的技术领域，具体涉及一种适用于双机器人系统的防碰撞方法。
背景技术
伴随着技术的不断发展，工业4.0以及中国智能制造2025的不断推进，机器人在各行各业得到了广泛的应用。随着机器人应用的不断深入，单台机器人在某些特性场景下无法满足使用需求，在此情景下双机器人乃至多机器人系统应运而生。双机器人系统具有比单机器人系统更大的承载能力以及更多的应用场景。在双机器人系统运行过程中，机器人之间不仅要按照各自设定好的运行轨迹进行运动，同时还需要实现机器人之间的位置感知和安全识别，尤其是机器人之间不能发生运动碰撞。因此，本发明提出了一种适用于双机器人系统的防碰撞控制方法，用以实现双机器人系统机器人之间的防碰撞控制。
发明内容
本发明的目的在于提供一种适用于双机器人系统的防碰撞方法，旨在解决双机器人系统机器人之间的位置感知和防碰撞控制。
本发明主要通过以下技术方案实现：
一种适用于双机器人系统的防碰撞方法，构建机器人各连杆在同一坐标系下的连杆模型，通过控制系统实时采集工业机器人各关节角度，通过连杆模型实时计算出工业机器人各连杆在空间内的位置，实时检测两台工业机器人连杆之间的最小距离，当最小距离小于或等于设定的阈值时，则控制系统发出碰撞警示信号，并控制工业机器人停止运动。
为了更好地实现本发明，进一步地，包括工业机器人A和工业机器人B，所述工业机器人A和工业机器人B分别设置有第一转轴至第六转轴，且第一转轴轴线、第二转轴轴线、第三转轴轴线、第五转轴轴线分别与X轴重合，第四转轴轴线、第六转轴轴线分别与Y轴重合，在第二转轴至第六转轴处分别建立坐标系，且各坐标系的坐标轴之间相互平行，工业机器人A和工业机器人B的基坐标系的Z轴与第一转轴轴线重合；包括以下步骤：
步骤S100：建立工业机器人A各连杆在工业机器人A基坐标系
Figure DEST_PATH_IMAGE001
的模型；
步骤S200：建立工业机器人B各连杆在工业机器人B基坐标系
Figure DEST_PATH_IMAGE002
的模型；
步骤S300：通过现场标定求解出工业机器人A与工业机器人B的基坐标系之间的转换关系为
Figure DEST_PATH_IMAGE003
，因此，建立工业机器人B各连杆在工业机器人A基坐标系
Figure 137805DEST_PATH_IMAGE001
下的模型；
步骤S400：根据步骤S100和步骤300的模型，实时计算出工业机器人A与工业机器人B各连杆在空间内的位置，对工业机器人A和工业机器人B的连杆之间的最小距离进行求解，当最小距离小于设定阈值时，则认定工业机器人A和工业机器人B的距离过近，进行防碰撞控制。
为了更好地实现本发明，进一步地，所述步骤S100包括以下步骤：
步骤S101：建立工业机器人A的第一连杆模型：
工业机器人A的第一连杆轴线在基坐标系
Figure 110309DEST_PATH_IMAGE001
下的方程如下：
Figure DEST_PATH_IMAGE004
步骤S102：建立工业机器人A的第二连杆模型：
工业机器人A的第二连杆轴线在第二转轴坐标系
Figure DEST_PATH_IMAGE005
下的方程如下：
Figure DEST_PATH_IMAGE006
将其转换到基坐标系
Figure 112505DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE007
其中：
Figure DEST_PATH_IMAGE008
步骤S103：建立工业机器人A的第三连杆模型：
工业机器人A第三连杆轴线在坐标系
Figure DEST_PATH_IMAGE009
下的方程：
Figure DEST_PATH_IMAGE010
将其转换到基坐标系
Figure 461447DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE011
其中：
Figure DEST_PATH_IMAGE012
步骤S104：建立工业机器人A的第四连杆模型：
工业机器人A第四连杆轴线在坐标系
Figure DEST_PATH_IMAGE013
下的方程为
Figure DEST_PATH_IMAGE014
将其转换到基坐标系
Figure 850840DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE015
步骤S105：建立工业机器人A的第五连杆模型：
工业机器人A的第五连杆在坐标系
Figure DEST_PATH_IMAGE016
下的方程为：
Figure DEST_PATH_IMAGE017
将其转换到基坐标系
Figure 322141DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE018
其中：
Figure DEST_PATH_IMAGE019
步骤S106：建立工业机器人A的第六连杆模型：
工业机器人A第六连杆在坐标系
Figure DEST_PATH_IMAGE020
下的方程为：
Figure DEST_PATH_IMAGE021
将其转换到基坐标系
Figure 173816DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE022
其中：l为连杆长度；
Figure DEST_PATH_IMAGE023
为工业机器人A的基坐标系
Figure 998553DEST_PATH_IMAGE001
与第二转轴坐标系
Figure DEST_PATH_IMAGE024
之间的距离；
Figure DEST_PATH_IMAGE025
为工业机器人A的第二转轴坐标系
Figure 307699DEST_PATH_IMAGE024
与第三转轴坐标系
Figure 825268DEST_PATH_IMAGE009
之间的距离；
Figure DEST_PATH_IMAGE026
为工业机器人A的第三转轴坐标系
Figure 725091DEST_PATH_IMAGE009
与第四转轴坐标系
Figure 353518DEST_PATH_IMAGE013
之间的距离；
Figure DEST_PATH_IMAGE027
为工业机器人A的第四转轴坐标系
Figure 389607DEST_PATH_IMAGE013
与第五转轴坐标系
Figure 78077DEST_PATH_IMAGE016
之间的距离；
Figure DEST_PATH_IMAGE028
为工业机器人A的第五转轴坐标系
Figure 465196DEST_PATH_IMAGE016
与第六转轴坐标系
Figure 569419DEST_PATH_IMAGE020
之间的距离；
Figure DEST_PATH_IMAGE029
为工业机器人A的第六转轴坐标系
Figure 787910DEST_PATH_IMAGE020
与工业机器人A法兰端面之间的距离；
Figure DEST_PATH_IMAGE030
为工业机器人A的第一转轴转动角度，
Figure DEST_PATH_IMAGE031
为工业机器人A的第二转轴转动角度，
Figure DEST_PATH_IMAGE032
为工业机器人A的第三转轴转动角度，
Figure DEST_PATH_IMAGE033
为工业机器人A的第四转轴转动角度，
Figure DEST_PATH_IMAGE034
为工业机器人A的第五转轴转动角度。
为了更好地实现本发明，进一步地，所述步骤S200包括以下步骤：
步骤S201：建立工业机器人B的第一连杆模型：
Figure DEST_PATH_IMAGE035
步骤S202：建立工业机器人B的第二连杆模型：
Figure DEST_PATH_IMAGE036
其中：
Figure DEST_PATH_IMAGE037
步骤S203：建立工业机器人B的第三连杆模型：
Figure DEST_PATH_IMAGE038
其中：
Figure DEST_PATH_IMAGE039
步骤S204：建立工业机器人B的第四连杆模型：
Figure DEST_PATH_IMAGE040
步骤S205：建立工业机器人B的第五连杆模型：
Figure DEST_PATH_IMAGE041
其中：
Figure DEST_PATH_IMAGE042
步骤S206：建立工业机器人B的第六连杆模型：
Figure DEST_PATH_IMAGE043
其中：l为连杆长度；
Figure DEST_PATH_IMAGE044
为工业机器人B的基坐标系
Figure DEST_PATH_IMAGE045
与第二转轴坐标系
Figure DEST_PATH_IMAGE046
之间的距离；
Figure DEST_PATH_IMAGE047
为工业机器人B的第二转轴坐标系
Figure 961796DEST_PATH_IMAGE046
与第三转轴坐标系
Figure DEST_PATH_IMAGE048
之间的距离；
Figure DEST_PATH_IMAGE049
为工业机器人B的第三转轴坐标系
Figure 632949DEST_PATH_IMAGE048
与第四转轴坐标系
Figure DEST_PATH_IMAGE050
之间的距离；
Figure DEST_PATH_IMAGE051
为工业机器人B的第四转轴坐标系
Figure 806441DEST_PATH_IMAGE050
与第五转轴坐标系
Figure DEST_PATH_IMAGE052
之间的距离；
Figure DEST_PATH_IMAGE053
为工业机器人B的第五转轴坐标系
Figure 941756DEST_PATH_IMAGE052
与第六转轴坐标系
Figure DEST_PATH_IMAGE054
之间的距离；
Figure DEST_PATH_IMAGE055
为工业机器人B的第六转轴坐标系
Figure 972029DEST_PATH_IMAGE054
与工业机器人B法兰端面之间的距离；
Figure DEST_PATH_IMAGE056
为工业机器人B的第一转轴转动角度，
Figure DEST_PATH_IMAGE057
为工业机器人B的第二转轴转动角度，
Figure DEST_PATH_IMAGE058
为工业机器人B的第三转轴转动角度，
Figure DEST_PATH_IMAGE059
为工业机器人B的第四转轴转动角度，
Figure DEST_PATH_IMAGE060
为工业机器人B的第五转轴转动角度。
为了更好地实现本发明，进一步地，所述步骤S300包括以下步骤：
步骤S301：转换后工业机器人B的第一连杆模型：
Figure DEST_PATH_IMAGE061
步骤S302：转换后工业机器人B的第二连杆模型：
Figure DEST_PATH_IMAGE062
步骤S303：转换后工业机器人B的第三连杆模型：
Figure DEST_PATH_IMAGE063
步骤S304：转换后工业机器人B的第四连杆模型：
Figure DEST_PATH_IMAGE064
步骤S305：转换后工业机器人B的第五连杆模型：
Figure DEST_PATH_IMAGE065
步骤S306：转换后工业机器人B的第六连杆模型：
Figure DEST_PATH_IMAGE066
。
为了更好地实现本发明，进一步地，工业机器人A和工业机器人B任意连杆之间的最小距离计算如下：
工业机器人A任一连杆的方程为：
Figure DEST_PATH_IMAGE067
其中，
Figure DEST_PATH_IMAGE068
为工业机器人A对应连杆的长度；
Figure DEST_PATH_IMAGE069
为工业机器人A对应连杆的最大长度；
工业机器人A任一连杆上点的坐标为（xA，yA，zA），
a 1为工业机器人A任一连杆的X坐标轴的变系数，与转轴转动角度相关，
b 1为工业机器人A任一连杆的X坐标轴的变常数，与转轴转动角度相关，
c 1为工业机器人A任一连杆的Y坐标轴的变系数，与转轴转动角度相关，
d 1为工业机器人A任一连杆的Y坐标轴的变常数，与转轴转动角度相关，
e 1为工业机器人A任一连杆的Z坐标轴的变系数，与转轴转动角度相关，
f 1为工业机器人A任一连杆的Z坐标轴的变常数，与转轴转动角度相关，
工业机器人B任一连杆的方程为：
Figure DEST_PATH_IMAGE070
其中，
Figure DEST_PATH_IMAGE071
为工业机器人B对应连杆的长度；
Figure DEST_PATH_IMAGE072
为工业机器人B对应连杆的最大长度；
工业机器人B任一连杆上点的坐标为（xB，yB，zB），
a 2为工业机器人B任一连杆的X坐标轴的变系数，与转轴转动角度相关，
b 2为工业机器人B任一连杆的X坐标轴的变常数，与转轴转动角度相关，
c 2为工业机器人B任一连杆的Y坐标轴的变系数，与转轴转动角度相关，
d 2为工业机器人B任一连杆的Y坐标轴的变常数，与转轴转动角度相关，
e 2为工业机器人B任一连杆的Z坐标轴的变系数，与转轴转动角度相关，
f 2为工业机器人B任一连杆的Z坐标轴的变常数，与转轴转动角度相关，
则工业机器人A和工业机器人B的两个连杆任意两点之间的距离为：
Figure DEST_PATH_IMAGE073
令：
Figure DEST_PATH_IMAGE074
先求得函数
Figure DEST_PATH_IMAGE075
取最小值，对函数求
Figure 620224DEST_PATH_IMAGE075
偏导，并令导数等于零：
Figure DEST_PATH_IMAGE076
则，若上述方程组
Figure DEST_PATH_IMAGE077
有解，则工业机器人A和工业机器人B的连杆之间的最小距离为：
Figure DEST_PATH_IMAGE078
否则，工业机器人A和工业机器人B的连杆之间的最小距离为工业机器人A和工业机器人B的连杆端点之间距离的最小值。
本发明的有益效果：
本发明构建了关于工业机器人的数学模型，通过控制系统实时采集机器人各关节角度，驱动工业机器人模型，实时计算出工业机器人各连杆在空间内的位置，进而实现实时检测两台工业机器人连杆之间的最小距离，然后将该最小距离与设定阈值进行比较，实现实时的对工业机器人进行防碰撞控制，具有较好的实用性。
附图说明
图1为双机器人系统示意图；
图2为双机器人系统的简化模型示意图。
具体实施方式
实施例1：
一种适用于双机器人系统的防碰撞方法，构建机器人各连杆在同一坐标系下的连杆模型，通过控制系统实时采集工业机器人各关节角度，通过连杆模型实时计算出工业机器人各连杆在空间内的位置，实时检测两台工业机器人连杆之间的最小距离，当最小距离小于或等于设定的阈值时，则控制系统发出碰撞警示信号，并控制工业机器人停止运动。
本发明构建了关于工业机器人的数学模型，通过控制系统实时采集机器人各关节角度，驱动工业机器人模型，实时计算出工业机器人各连杆在空间内的位置，进而实现实时检测两台工业机器人连杆之间的最小距离，然后将该最小距离与设定阈值进行比较，实现实时的对工业机器人进行防碰撞控制，具有较好的实用性。
实施例2：
本实施例是在实施例1的基础上进行优化，如图1、图2所示，包括工业机器人A和工业机器人B，所述工业机器人A和工业机器人B分别设置有第一转轴至第六转轴，且第一转轴轴线、第二转轴轴线、第三转轴轴线、第五转轴轴线分别与X轴重合，第四转轴轴线、第六转轴轴线分别与Y轴重合，在第二转轴至第六转轴处分别建立坐标系，且各坐标系的坐标轴之间相互平行，工业机器人A和工业机器人B的基坐标系的Z轴与第一转轴轴线重合；包括以下步骤：
步骤S100：建立工业机器人A各连杆在工业机器人A基坐标系
Figure 131496DEST_PATH_IMAGE001
的模型；
步骤S200：同步骤S100的方法，建立工业机器人B各连杆在工业机器人B基坐标系
Figure 324580DEST_PATH_IMAGE002
的模型；
步骤S300：通过现场标定求解出工业机器人A与工业机器人B的基坐标系之间的转换关系为
Figure 463437DEST_PATH_IMAGE003
，因此，建立工业机器人B各连杆在工业机器人A基坐标系
Figure 46865DEST_PATH_IMAGE001
下的模型；
步骤S400：根据步骤S100和步骤300的模型，实时计算出工业机器人A与工业机器人B各连杆在空间内的位置，对工业机器人A和工业机器人B的连杆之间的最小距离进行求解，当最小距离小于设定阈值时，则认定工业机器人A和工业机器人B的距离过近，进行防碰撞控制。
进一步地，工业机器人A和工业机器人B任意连杆之间的最小距离计算如下：
工业机器人A任一连杆的方程为：
Figure 358898DEST_PATH_IMAGE067
其中，
Figure 344171DEST_PATH_IMAGE068
为工业机器人A对应连杆的长度；
Figure 716247DEST_PATH_IMAGE069
为工业机器人A对应连杆的最大长度；
工业机器人A任一连杆上点的坐标为（xA，yA，zA），
a 1为工业机器人A任一连杆的X坐标轴的变系数，与转轴转动角度相关，
b 1为工业机器人A任一连杆的X坐标轴的变常数，与转轴转动角度相关，
c 1为工业机器人A任一连杆的Y坐标轴的变系数，与转轴转动角度相关，
d 1为工业机器人A任一连杆的Y坐标轴的变常数，与转轴转动角度相关，
e 1为工业机器人A任一连杆的Z坐标轴的变系数，与转轴转动角度相关，
f 1为工业机器人A任一连杆的Z坐标轴的变常数，与转轴转动角度相关，
工业机器人B任一连杆的方程为：
Figure 786971DEST_PATH_IMAGE070
其中，
Figure 902694DEST_PATH_IMAGE071
为工业机器人B对应连杆的长度；
Figure 742474DEST_PATH_IMAGE072
为工业机器人B对应连杆的最大长度；
工业机器人B任一连杆上点的坐标为（xB，yB，zB），
a 2为工业机器人B任一连杆的X坐标轴的变系数，与转轴转动角度相关，
b 2为工业机器人B任一连杆的X坐标轴的变常数，与转轴转动角度相关，
c 2为工业机器人B任一连杆的Y坐标轴的变系数，与转轴转动角度相关，
d 2为工业机器人B任一连杆的Y坐标轴的变常数，与转轴转动角度相关，
e 2为工业机器人B任一连杆的Z坐标轴的变系数，与转轴转动角度相关，
f 2为工业机器人B任一连杆的Z坐标轴的变常数，与转轴转动角度相关，
则工业机器人A和工业机器人B的两个连杆任意两点之间的距离为：
Figure 223134DEST_PATH_IMAGE073
令：
Figure DEST_PATH_IMAGE079
先求得函数
Figure 577892DEST_PATH_IMAGE075
取最小值，对函数求
Figure 434990DEST_PATH_IMAGE075
偏导，并令导数等于零：
Figure 129276DEST_PATH_IMAGE076
则，若上述方程组
Figure 843155DEST_PATH_IMAGE077
有解，则工业机器人A和工业机器人B的连杆之间的最小距离为：
Figure DEST_PATH_IMAGE080
否则，工业机器人A和工业机器人B的连杆之间的最小距离为工业机器人A和工业机器人B的连杆端点之间距离的最小值。
本发明构建了关于工业机器人的数学模型，通过控制系统实时采集机器人各关节角度，驱动工业机器人模型，实时计算出工业机器人各连杆在空间内的位置，进而实现实时检测两台工业机器人连杆之间的最小距离，然后将该最小距离与设定阈值进行比较，实现实时的对工业机器人进行防碰撞控制，具有较好的实用性。
本实施例的其他部分与实施例1相同，故不再赘述。
实施例3：
本实施例是在实施例1或2的基础上进行优化，所述步骤S100包括以下步骤：
步骤S101：建立工业机器人A的第一连杆模型：
工业机器人A的第一连杆轴线在基坐标系
Figure 950788DEST_PATH_IMAGE001
下的方程如下：
Figure 345997DEST_PATH_IMAGE004
步骤S102：建立工业机器人A的第二连杆模型：
工业机器人A的第二连杆轴线在第二转轴坐标系
Figure 894790DEST_PATH_IMAGE005
下的方程如下：
Figure 48078DEST_PATH_IMAGE006
将其转换到基坐标系
Figure 315112DEST_PATH_IMAGE001
下，方程为：
Figure 248433DEST_PATH_IMAGE007
其中：
Figure DEST_PATH_IMAGE081
步骤S103：建立工业机器人A的第三连杆模型：
工业机器人A第三连杆轴线在坐标系
Figure 979628DEST_PATH_IMAGE009
下的方程：
Figure 238571DEST_PATH_IMAGE010
将其转换到基坐标系
Figure 727321DEST_PATH_IMAGE001
下，方程为：
Figure 100002_DEST_PATH_IMAGE082
其中：
Figure 792229DEST_PATH_IMAGE012
步骤S104：建立工业机器人A的第四连杆模型：
工业机器人A第四连杆轴线在坐标系
Figure 315615DEST_PATH_IMAGE013
下的方程为
Figure 479880DEST_PATH_IMAGE014
将其转换到基坐标系
Figure 721505DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE083
步骤S105：建立工业机器人A的第五连杆模型：
工业机器人A的第五连杆在坐标系
Figure 324525DEST_PATH_IMAGE016
下的方程为：
Figure 764733DEST_PATH_IMAGE017
将其转换到基坐标系
Figure 365479DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE084
其中：
Figure DEST_PATH_IMAGE085
步骤S106：建立工业机器人A的第六连杆模型：
工业机器人A第六连杆在坐标系
Figure 891138DEST_PATH_IMAGE020
下的方程为：
Figure 235532DEST_PATH_IMAGE021
将其转换到基坐标系
Figure 858143DEST_PATH_IMAGE001
下，方程为：
Figure 100002_DEST_PATH_IMAGE086
其中：l为连杆长度；
Figure 364211DEST_PATH_IMAGE023
为工业机器人A的基坐标系
Figure 580429DEST_PATH_IMAGE001
与第二转轴坐标系
Figure 462934DEST_PATH_IMAGE024
之间的距离；
Figure 815418DEST_PATH_IMAGE025
为工业机器人A的第二转轴坐标系
Figure 492387DEST_PATH_IMAGE024
与第三转轴坐标系
Figure 930322DEST_PATH_IMAGE009
之间的距离；
Figure 806398DEST_PATH_IMAGE026
为工业机器人A的第三转轴坐标系
Figure 13389DEST_PATH_IMAGE009
与第四转轴坐标系
Figure 126838DEST_PATH_IMAGE013
之间的距离；
Figure 52069DEST_PATH_IMAGE027
为工业机器人A的第四转轴坐标系
Figure 338694DEST_PATH_IMAGE013
与第五转轴坐标系
Figure 665770DEST_PATH_IMAGE016
之间的距离；
Figure 950121DEST_PATH_IMAGE028
为工业机器人A的第五转轴坐标系
Figure 97068DEST_PATH_IMAGE016
与第六转轴坐标系
Figure 187384DEST_PATH_IMAGE020
之间的距离；
Figure 368967DEST_PATH_IMAGE029
为工业机器人A的第六转轴坐标系
Figure 886536DEST_PATH_IMAGE020
与工业机器人A法兰端面之间的距离；
Figure 786358DEST_PATH_IMAGE030
为工业机器人A的第一转轴转动角度，
Figure 414786DEST_PATH_IMAGE031
为工业机器人A的第二转轴转动角度，
Figure 450875DEST_PATH_IMAGE032
为工业机器人A的第三转轴转动角度，
Figure 77028DEST_PATH_IMAGE033
为工业机器人A的第四转轴转动角度，
Figure 260885DEST_PATH_IMAGE034
为工业机器人A的第五转轴转动角度。
本实施例的其他部分与上述实施例1或2相同，故不再赘述。
实施例4：
本实施例是在实施例2或3的基础上进行优化，所述步骤S200包括以下步骤：
步骤S201：建立工业机器人B的第一连杆模型：
Figure 630687DEST_PATH_IMAGE035
步骤S202：建立工业机器人B的第二连杆模型：
Figure 863827DEST_PATH_IMAGE036
其中：
Figure DEST_PATH_IMAGE087
步骤S203：建立工业机器人B的第三连杆模型：
Figure 100002_DEST_PATH_IMAGE088
其中：
Figure 723198DEST_PATH_IMAGE039
步骤S204：建立工业机器人B的第四连杆模型：
Figure 100002_DEST_PATH_IMAGE089
步骤S205：建立工业机器人B的第五连杆模型：
Figure DEST_PATH_IMAGE090
其中：
Figure DEST_PATH_IMAGE091
步骤S206：建立工业机器人B的第六连杆模型：
Figure DEST_PATH_IMAGE092
其中：l为连杆长度；
Figure 394351DEST_PATH_IMAGE044
为工业机器人B的基坐标系
Figure 302264DEST_PATH_IMAGE045
与第二转轴坐标系
Figure 312946DEST_PATH_IMAGE046
之间的距离；
Figure 280902DEST_PATH_IMAGE047
为工业机器人B的第二转轴坐标系
Figure 439350DEST_PATH_IMAGE046
与第三转轴坐标系
Figure 150954DEST_PATH_IMAGE048
之间的距离；
Figure 281722DEST_PATH_IMAGE049
为工业机器人B的第三转轴坐标系
Figure 420579DEST_PATH_IMAGE048
与第四转轴坐标系
Figure 800745DEST_PATH_IMAGE050
之间的距离；
Figure 378356DEST_PATH_IMAGE051
为工业机器人B的第四转轴坐标系
Figure 363630DEST_PATH_IMAGE050
与第五转轴坐标系
Figure 673389DEST_PATH_IMAGE052
之间的距离；
Figure 478534DEST_PATH_IMAGE053
为工业机器人B的第五转轴坐标系
Figure 594257DEST_PATH_IMAGE052
与第六转轴坐标系
Figure 434037DEST_PATH_IMAGE054
之间的距离；
Figure 977014DEST_PATH_IMAGE055
为工业机器人B的第六转轴坐标系
Figure 535034DEST_PATH_IMAGE054
与工业机器人B法兰端面之间的距离；
Figure 191799DEST_PATH_IMAGE056
为工业机器人B的第一转轴转动角度，
Figure 151665DEST_PATH_IMAGE057
为工业机器人B的第二转轴转动角度，
Figure 865543DEST_PATH_IMAGE058
为工业机器人B的第三转轴转动角度，
Figure 645280DEST_PATH_IMAGE059
为工业机器人B的第四转轴转动角度，
Figure 40489DEST_PATH_IMAGE060
为工业机器人B的第五转轴转动角度。
进一步地，所述步骤S300包括以下步骤：
步骤S301：转换后工业机器人B的第一连杆模型：
Figure 854862DEST_PATH_IMAGE061
步骤S302：转换后工业机器人B的第二连杆模型：
Figure 677324DEST_PATH_IMAGE062
步骤S303：转换后工业机器人B的第三连杆模型：
Figure DEST_PATH_IMAGE093
步骤S304：转换后工业机器人B的第四连杆模型：
Figure DEST_PATH_IMAGE094
步骤S305：转换后工业机器人B的第五连杆模型：
Figure DEST_PATH_IMAGE095
步骤S306：转换后工业机器人B的第六连杆模型：
Figure DEST_PATH_IMAGE096
。
本实施例的其他部分与上述实施例2或3相同，故不再赘述。
实施例5：
一种适用于双机器人系统的防碰撞方法，通过对双机器人系统工业机器人进行模型简化，构建关于工业机器人的数学模型。通过控制系统实时采集机器人各关节角度，驱动工业机器人模型，实时计算出工业机器人各连杆在空间内的位置。通过实时检测两台工业机器人连杆之间的最小距离，当最小距离小于或等于设定的安全距离时，控制系统发出碰撞警示信号，并控制机器人停止运动。
如图1、图2所示，
在工业机器人A简化模型中，坐标系为机器人基坐标系
Figure 131308DEST_PATH_IMAGE001
，其中Z轴与机器人第一转轴轴线重合；坐标系
Figure 330208DEST_PATH_IMAGE005
为机器人第二转轴坐标系，其中第二转轴轴线与X轴重合；坐标系
Figure 999087DEST_PATH_IMAGE009
为机器人第三转轴坐标系，其中第三转轴轴线与X轴重合；坐标系
Figure 992451DEST_PATH_IMAGE013
为机器人第四转轴坐标系，其中第四转轴轴线与Y轴重合；坐标系
Figure 746780DEST_PATH_IMAGE016
为机器人第五转轴坐标系，其中第五转轴轴线与X轴重合；坐标系
Figure 811688DEST_PATH_IMAGE020
为机器人第六转轴坐标系，其中第六转轴轴线与Y轴重合。同时上述六个坐标系各坐标轴之间相互平行。
Figure 397390DEST_PATH_IMAGE023
代表坐标系
Figure 561655DEST_PATH_IMAGE001
与坐标系
Figure 803281DEST_PATH_IMAGE024
之间的距离，即第一连杆长度；
Figure 343984DEST_PATH_IMAGE025
代表坐标系
Figure 721875DEST_PATH_IMAGE024
与坐标系
Figure 384938DEST_PATH_IMAGE009
之间的距离，即第二连杆长度；
Figure 848280DEST_PATH_IMAGE026
代表坐标系
Figure 192674DEST_PATH_IMAGE009
与坐标系
Figure 425072DEST_PATH_IMAGE013
之间的距离，即第三连杆长度；
Figure 261966DEST_PATH_IMAGE027
代表坐标系
Figure 212604DEST_PATH_IMAGE013
与坐标系
Figure 157426DEST_PATH_IMAGE016
之间的距离，即第四连杆长度；
Figure 509910DEST_PATH_IMAGE028
代表坐标系
Figure 514775DEST_PATH_IMAGE016
与坐标系
Figure 952710DEST_PATH_IMAGE020
之间的距离，即第五连杆长度；
Figure 638906DEST_PATH_IMAGE029
代表坐标系
Figure 845897DEST_PATH_IMAGE020
与机器人法兰端面的距离，即第六连杆长度。
Figure 21663DEST_PATH_IMAGE030
代表第一转轴转动角度，
Figure 946894DEST_PATH_IMAGE031
代表第二转轴转动角度，
Figure 171202DEST_PATH_IMAGE032
代表第三转轴转动角度，
Figure 498278DEST_PATH_IMAGE033
代表第四转轴转动角度，
Figure 517049DEST_PATH_IMAGE034
代表第五转轴转动角度，
Figure DEST_PATH_IMAGE097
代表第六转轴转动角度。
图2中，在工业机器人B简化模型中，坐标系
Figure 929576DEST_PATH_IMAGE045
为机器人基坐标系，其中Z轴与机器人第一转轴轴线重合；坐标系
Figure 19892DEST_PATH_IMAGE046
为机器人第二转轴坐标系，其中第二转轴轴线与X轴重合；坐标系
Figure 263792DEST_PATH_IMAGE048
为机器人第三转轴坐标系，其中第三转轴轴线与X轴重合；坐标系
Figure 719044DEST_PATH_IMAGE050
为机器人第四转轴坐标系，其中第四转轴轴线与Y轴重合；坐标系
Figure 353287DEST_PATH_IMAGE052
为机器人第五转轴坐标系，其中第五转轴轴线与X轴重合；坐标系
Figure 919398DEST_PATH_IMAGE054
为机器人第六转轴坐标系，其中第六转轴轴线与Y轴重合。同时上述六个坐标系各坐标轴之间相互平行。
Figure 221066DEST_PATH_IMAGE044
代表坐标系
Figure 847220DEST_PATH_IMAGE045
与坐标系
Figure 968759DEST_PATH_IMAGE046
之间的距离，
Figure 463195DEST_PATH_IMAGE047
代表坐标系
Figure 619369DEST_PATH_IMAGE046
与坐标系
Figure 416424DEST_PATH_IMAGE048
之间的距离，
Figure 25260DEST_PATH_IMAGE049
代表坐标系
Figure 998420DEST_PATH_IMAGE048
与坐标系
Figure 9101DEST_PATH_IMAGE050
之间的距离，
Figure 977057DEST_PATH_IMAGE051
代表坐标系
Figure 135506DEST_PATH_IMAGE050
与坐标系
Figure 909427DEST_PATH_IMAGE052
之间的距离，
Figure 774615DEST_PATH_IMAGE053
代表坐标系
Figure 913472DEST_PATH_IMAGE052
与坐标系
Figure 496900DEST_PATH_IMAGE054
之间的距离，
Figure 136829DEST_PATH_IMAGE055
代表坐标系
Figure 856523DEST_PATH_IMAGE054
与机器人法兰端面的距离。
Figure 166282DEST_PATH_IMAGE056
代表第一转轴转动角度，
Figure 237006DEST_PATH_IMAGE057
代表第二转轴转动角度，
Figure 290413DEST_PATH_IMAGE058
代表第三转轴转动角度，
Figure 192510DEST_PATH_IMAGE059
代表第四转轴转动角度，
Figure 673169DEST_PATH_IMAGE060
代表第五转轴转动角度，
Figure DEST_PATH_IMAGE098
代表第六转轴转动角度。
进一步地，机器人模型构建方法如下：
步骤一：建立工业机器人第一连杆模型。
如图2所示，工业机器人A第一连杆轴线在基坐标系
Figure 231190DEST_PATH_IMAGE001
下的方程为：
Figure 885025DEST_PATH_IMAGE004
步骤二：建立工业机器人第二连杆模型。
如图2所示，工业机器人A第二连杆轴线在坐标系
Figure 579312DEST_PATH_IMAGE001
下的方程为：
Figure 496452DEST_PATH_IMAGE006
将其转换到基坐标系
Figure 276189DEST_PATH_IMAGE001
下，方程为：
Figure 733715DEST_PATH_IMAGE007
其中：
Figure 548088DEST_PATH_IMAGE081
步骤三：建立工业机器人第三连杆模型。
如图2所示，工业机器人A第三连杆轴线在坐标系
Figure 370550DEST_PATH_IMAGE001
下的方程为：
Figure 637583DEST_PATH_IMAGE010
将其转换到基坐标系
Figure 952328DEST_PATH_IMAGE001
下，方程为：
Figure 621207DEST_PATH_IMAGE082
其中：
Figure 614571DEST_PATH_IMAGE012
步骤四：建立工业机器人第四连杆模型。
如图2所示，工业机器人A第四连杆轴线在坐标系
Figure 368900DEST_PATH_IMAGE001
下的方程为：
Figure 168229DEST_PATH_IMAGE014
将其转换到基坐标系
Figure 753931DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE099
步骤五：建立工业机器人第五连杆模型。
如图2所示，工业机器人A第五连杆在坐标系
Figure 183775DEST_PATH_IMAGE001
下的方程为：
工业机器人A的第五连杆在坐标系
Figure 425401DEST_PATH_IMAGE016
下的方程为：
Figure 700524DEST_PATH_IMAGE017
将其转换到基坐标系
Figure 343995DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE100
其中：
Figure 803795DEST_PATH_IMAGE085
步骤六：建立工业机器人第六连杆模型。
如图2所示，工业机器人A第六连杆在坐标系
Figure 532717DEST_PATH_IMAGE001
下的方程为：
工业机器人A第六连杆在坐标系
Figure 611531DEST_PATH_IMAGE020
下的方程为：
Figure 109509DEST_PATH_IMAGE021
将其转换到基坐标系
Figure 943473DEST_PATH_IMAGE001
下，方程为：
Figure DEST_PATH_IMAGE101
进一步地，同理，可以得到工业机器人B各连杆在其基坐标系
Figure 956428DEST_PATH_IMAGE045
的方程如下：
第一连杆模型为：
Figure 838933DEST_PATH_IMAGE035
第二连杆模型为：
Figure 256664DEST_PATH_IMAGE036
其中：
Figure DEST_PATH_IMAGE102
第三连杆模型为：
Figure DEST_PATH_IMAGE103
其中：
Figure 199212DEST_PATH_IMAGE039
第四连杆模型为：
Figure DEST_PATH_IMAGE104
第五连杆模型为：
Figure DEST_PATH_IMAGE105
其中：
Figure 761781DEST_PATH_IMAGE106
第六连杆模型为：
Figure 182398DEST_PATH_IMAGE107
进一步地，通过现场标定可以求解出工业机器人A与工业机器人B基坐标系之间的转换关系为
Figure 717284DEST_PATH_IMAGE003
，因此工业机器人B各连杆在工业机器人A基坐标系
Figure 830734DEST_PATH_IMAGE001
下的方程为：
第一连杆模型为：
Figure DEST_PATH_IMAGE108
第二连杆模型为：
Figure 818281DEST_PATH_IMAGE062
第三连杆模型为：
Figure 42589DEST_PATH_IMAGE109
第四连杆模型为：
Figure 166403DEST_PATH_IMAGE110
第五连杆模型为：
Figure DEST_PATH_IMAGE111
第六连杆模型为：
Figure 513071DEST_PATH_IMAGE112
上述工业机器人A和工业机器人B各连杆方程求解完毕之后，对两台工业机器人连杆之间的最短距离进行求解，到最短距离小于对应连杆半径以及安全距离之和时，即可认定机器人距离过近，进行防碰撞控制。
进一步地，工业机器人A和工业机器人B任意连杆之间的最短距离一般计算方法如下：
工业机器人A某一连杆一般方程为：
Figure DEST_PATH_IMAGE113
工业机器人B某一连杆一般方程为：
Figure 925597DEST_PATH_IMAGE114
两连杆任意两点之间的距离为：
Figure 953596DEST_PATH_IMAGE073
令：
Figure 135179DEST_PATH_IMAGE074
为求得两连杆之间的最短距离，可先求得函数
Figure 387169DEST_PATH_IMAGE075
取得最小值。
对函数
Figure 286992DEST_PATH_IMAGE075
求偏导，并令导数等于零。
Figure DEST_PATH_IMAGE115
如果上述方程组有解
Figure 853102DEST_PATH_IMAGE077
，则两连杆之间的最短距离为：
Figure 220017DEST_PATH_IMAGE080
当上述方程组无解时，两连杆之间的最短距离为连杆端点之间距离的最小值。
其中：
工业机器人A任一连杆上点的坐标为（xA，yA，zA），
a 1为工业机器人A任一连杆的X坐标轴的变系数，与转轴转动角度相关，
b 1为工业机器人A任一连杆的X坐标轴的变常数，与转轴转动角度相关，
c 1为工业机器人A任一连杆的Y坐标轴的变系数，与转轴转动角度相关，
d 1为工业机器人A任一连杆的Y坐标轴的变常数，与转轴转动角度相关，
e 1为工业机器人A任一连杆的Z坐标轴的变系数，与转轴转动角度相关，
f 1为工业机器人A任一连杆的Z坐标轴的变常数，与转轴转动角度相关，
工业机器人B任一连杆上点的坐标为（xB，yB，zB），
a 2为工业机器人B任一连杆的X坐标轴的变系数，与转轴转动角度相关，
b 2为工业机器人B任一连杆的X坐标轴的变常数，与转轴转动角度相关，
c 2为工业机器人B任一连杆的Y坐标轴的变系数，与转轴转动角度相关，
d 2为工业机器人B任一连杆的Y坐标轴的变常数，与转轴转动角度相关，
e 2为工业机器人B任一连杆的Z坐标轴的变系数，与转轴转动角度相关，
f 2为工业机器人B任一连杆的Z坐标轴的变常数，与转轴转动角度相关。
本发明可以依次求解出两台工业机器人各连杆之间的最短距离，并与连杆的半径以及安全距离进行比较，可以对工业机器人是否进行防碰撞控制进行判定，从而达到机器人运动过程中防碰撞控制的目的。
以上所述，仅是本发明的较佳实施例，并非对本发明做任何形式上的限制，凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化，均落入本发明的保护范围之内。

Claims (1)

1.一种适用于双机器人系统的防碰撞方法，其特征在于，构建机器人各连杆在同一坐标系下的连杆模型，通过控制系统实时采集工业机器人各关节角度，通过连杆模型实时计算出工业机器人各连杆在空间内的位置，实时检测两台工业机器人连杆之间的最小距离，当最小距离小于或等于设定的阈值时，则控制系统发出碰撞警示信号，并控制工业机器人停止运动；
包括工业机器人A和工业机器人B，所述工业机器人A和工业机器人B分别设置有第一转轴至第六转轴，且第一转轴轴线、第二转轴轴线、第三转轴轴线、第五转轴轴线分别与X轴重合，第四转轴轴线、第六转轴轴线分别与Y轴重合，在第二转轴至第六转轴处分别建立坐标系，且各坐标系的坐标轴之间相互平行，工业机器人A和工业机器人B的基坐标系的Z轴与第一转轴轴线重合；包括以下步骤：
步骤S100：建立工业机器人A各连杆在工业机器人A基坐标系
Figure 692288DEST_PATH_IMAGE001
的模型；
步骤S200：建立工业机器人B各连杆在工业机器人B基坐标系
Figure 688057DEST_PATH_IMAGE002
的模型；
步骤S300：通过现场标定求解出工业机器人A与工业机器人B的基坐标系之间的转换关系为
Figure 546422DEST_PATH_IMAGE003
，因此，建立工业机器人B各连杆在工业机器人A基坐标系
Figure 472790DEST_PATH_IMAGE001
下的模型；
步骤S400：根据步骤S100和步骤300的模型，实时计算出工业机器人A与工业机器人B各连杆在空间内的位置，对工业机器人A和工业机器人B的连杆之间的最小距离进行求解，当最小距离小于设定阈值时，则认定工业机器人A和工业机器人B的距离过近，进行防碰撞控制；
所述步骤S100包括以下步骤：
步骤S101：建立工业机器人A的第一连杆模型：
工业机器人A的第一连杆轴线在基坐标系
Figure 818321DEST_PATH_IMAGE001
下的方程如下：
Figure 171942DEST_PATH_IMAGE004
步骤S102：建立工业机器人A的第二连杆模型：
工业机器人A的第二连杆轴线在第二转轴坐标系
Figure 720866DEST_PATH_IMAGE005
下的方程如下：
Figure 185345DEST_PATH_IMAGE006
将其转换到基坐标系
Figure 385382DEST_PATH_IMAGE001
下，方程为：
Figure 909905DEST_PATH_IMAGE007
其中：
Figure 946125DEST_PATH_IMAGE008
步骤S103：建立工业机器人A的第三连杆模型：
工业机器人A第三连杆轴线在坐标系
Figure 214295DEST_PATH_IMAGE009
下的方程：
Figure 268839DEST_PATH_IMAGE010
将其转换到基坐标系
Figure 229842DEST_PATH_IMAGE001
下，方程为：
Figure 753358DEST_PATH_IMAGE012
其中：
Figure 559640DEST_PATH_IMAGE013
步骤S104：建立工业机器人A的第四连杆模型：
工业机器人A第四连杆轴线在坐标系
Figure 734269DEST_PATH_IMAGE014
下的方程为
Figure 600594DEST_PATH_IMAGE015
将其转换到基坐标系
Figure 611407DEST_PATH_IMAGE001
下，方程为：
Figure 221380DEST_PATH_IMAGE017
步骤S105：建立工业机器人A的第五连杆模型：
工业机器人A的第五连杆在坐标系
Figure 250515DEST_PATH_IMAGE018
下的方程为：
Figure 553321DEST_PATH_IMAGE019
将其转换到基坐标系
Figure 785850DEST_PATH_IMAGE001
下，方程为：
Figure 199514DEST_PATH_IMAGE021
其中：
Figure 83156DEST_PATH_IMAGE022
步骤S106：建立工业机器人A的第六连杆模型：
工业机器人A第六连杆在坐标系
Figure 556863DEST_PATH_IMAGE023
下的方程为：
Figure 73426DEST_PATH_IMAGE024
将其转换到基坐标系
Figure 25202DEST_PATH_IMAGE001
下，方程为：
Figure 28930DEST_PATH_IMAGE026
其中：l为连杆长度；
Figure 424270DEST_PATH_IMAGE027
为工业机器人A的基坐标系
Figure 880659DEST_PATH_IMAGE001
与第二转轴坐标系
Figure 839388DEST_PATH_IMAGE028
之间的距离；
Figure 963202DEST_PATH_IMAGE029
为工业机器人A的第二转轴坐标系
Figure 529443DEST_PATH_IMAGE028
与第三转轴坐标系
Figure 473129DEST_PATH_IMAGE009
之间的距离；
Figure 32286DEST_PATH_IMAGE030
为工业机器人A的第三转轴坐标系
Figure 10606DEST_PATH_IMAGE009
与第四转轴坐标系
Figure 216591DEST_PATH_IMAGE014
之间的距离；
Figure 647572DEST_PATH_IMAGE031
为工业机器人A的第四转轴坐标系
Figure 744841DEST_PATH_IMAGE014
与第五转轴坐标系
Figure 843247DEST_PATH_IMAGE018
之间的距离；
Figure 485712DEST_PATH_IMAGE032
为工业机器人A的第五转轴坐标系
Figure 403990DEST_PATH_IMAGE018
与第六转轴坐标系
Figure 39370DEST_PATH_IMAGE023
之间的距离；
Figure 461124DEST_PATH_IMAGE033
为工业机器人A的第六转轴坐标系
Figure 540070DEST_PATH_IMAGE023
与工业机器人A法兰端面之间的距离；
Figure 945643DEST_PATH_IMAGE034
为工业机器人A的第一转轴转动角度，
Figure 650294DEST_PATH_IMAGE035
为工业机器人A的第二转轴转动角度，
Figure 926555DEST_PATH_IMAGE036
为工业机器人A的第三转轴转动角度，
Figure 176402DEST_PATH_IMAGE037
为工业机器人A的第四转轴转动角度，
Figure 803692DEST_PATH_IMAGE038
为工业机器人A的第五转轴转动角度；
所述步骤S200包括以下步骤：
步骤S201：建立工业机器人B的第一连杆模型：
Figure 46455DEST_PATH_IMAGE039
步骤S202：建立工业机器人B的第二连杆模型：
Figure 442801DEST_PATH_IMAGE040
其中：
Figure 851830DEST_PATH_IMAGE041
步骤S203：建立工业机器人B的第三连杆模型：
Figure 966417DEST_PATH_IMAGE043
其中：
Figure 544029DEST_PATH_IMAGE044
步骤S204：建立工业机器人B的第四连杆模型：
Figure 811193DEST_PATH_IMAGE046
步骤S205：建立工业机器人B的第五连杆模型：
Figure 652110DEST_PATH_IMAGE048
其中：
Figure 253993DEST_PATH_IMAGE050
步骤S206：建立工业机器人B的第六连杆模型：
Figure 838558DEST_PATH_IMAGE051
其中：l为连杆长度；
Figure 960229DEST_PATH_IMAGE052
为工业机器人B的基坐标系
Figure 972047DEST_PATH_IMAGE053
与第二转轴坐标系
Figure 530068DEST_PATH_IMAGE054
之间的距离；
Figure 918324DEST_PATH_IMAGE055
为工业机器人B的第二转轴坐标系
Figure 894501DEST_PATH_IMAGE054
与第三转轴坐标系
Figure 77221DEST_PATH_IMAGE056
之间的距离；
Figure 919275DEST_PATH_IMAGE057
为工业机器人B的第三转轴坐标系
Figure 845642DEST_PATH_IMAGE056
与第四转轴坐标系
Figure 410747DEST_PATH_IMAGE058
之间的距离；
Figure 29947DEST_PATH_IMAGE059
为工业机器人B的第四转轴坐标系
Figure 828139DEST_PATH_IMAGE058
与第五转轴坐标系
Figure 558197DEST_PATH_IMAGE060
之间的距离；
Figure 243388DEST_PATH_IMAGE061
为工业机器人B的第五转轴坐标系
Figure 830227DEST_PATH_IMAGE060
与第六转轴坐标系
Figure 115715DEST_PATH_IMAGE062
之间的距离；
Figure 587147DEST_PATH_IMAGE063
为工业机器人B的第六转轴坐标系
Figure 392424DEST_PATH_IMAGE062
与工业机器人B法兰端面之间的距离；
Figure 150164DEST_PATH_IMAGE064
为工业机器人B的第一转轴转动角度，
Figure 1576DEST_PATH_IMAGE065
为工业机器人B的第二转轴转动角度，
Figure 73438DEST_PATH_IMAGE066
为工业机器人B的第三转轴转动角度，
Figure 998800DEST_PATH_IMAGE067
为工业机器人B的第四转轴转动角度，
Figure 865124DEST_PATH_IMAGE068
为工业机器人B的第五转轴转动角度；
所述步骤S300包括以下步骤：
步骤S301：转换后工业机器人B的第一连杆模型：
Figure 656363DEST_PATH_IMAGE069
步骤S302：转换后工业机器人B的第二连杆模型：
Figure 17068DEST_PATH_IMAGE070
步骤S303：转换后工业机器人B的第三连杆模型：
Figure 46204DEST_PATH_IMAGE071
步骤S304：转换后工业机器人B的第四连杆模型：
Figure 83430DEST_PATH_IMAGE072
步骤S305：转换后工业机器人B的第五连杆模型：
Figure 830806DEST_PATH_IMAGE074
步骤S306：转换后工业机器人B的第六连杆模型：
Figure 995203DEST_PATH_IMAGE076
工业机器人A和工业机器人B任意连杆之间的最小距离计算如下：
工业机器人A任一连杆的方程为：
Figure 878845DEST_PATH_IMAGE077
其中，
Figure 86972DEST_PATH_IMAGE078
为工业机器人A对应连杆的长度；
Figure 56065DEST_PATH_IMAGE079
为工业机器人A对应连杆的最大长度；
工业机器人A任一连杆上点的坐标为（xA，yA，zA），
a 1为工业机器人A任一连杆的X坐标轴的变系数，与转轴转动角度相关，
b 1为工业机器人A任一连杆的X坐标轴的变常数，与转轴转动角度相关，
c 1为工业机器人A任一连杆的Y坐标轴的变系数，与转轴转动角度相关，
d 1为工业机器人A任一连杆的Y坐标轴的变常数，与转轴转动角度相关，
e 1为工业机器人A任一连杆的Z坐标轴的变系数，与转轴转动角度相关，
f 1为工业机器人A任一连杆的Z坐标轴的变常数，与转轴转动角度相关，
工业机器人B任一连杆的方程为：
Figure 758573DEST_PATH_IMAGE080
其中，
Figure 762301DEST_PATH_IMAGE081
为工业机器人B对应连杆的长度；
Figure DEST_PATH_IMAGE082
为工业机器人B对应连杆的最大长度；
工业机器人B任一连杆上点的坐标为（xB，yB，zB），
a 2为工业机器人B任一连杆的X坐标轴的变系数，与转轴转动角度相关，
b 2为工业机器人B任一连杆的X坐标轴的变常数，与转轴转动角度相关，
c 2为工业机器人B任一连杆的Y坐标轴的变系数，与转轴转动角度相关，
d 2为工业机器人B任一连杆的Y坐标轴的变常数，与转轴转动角度相关，
e 2为工业机器人B任一连杆的Z坐标轴的变系数，与转轴转动角度相关，
f 2为工业机器人B任一连杆的Z坐标轴的变常数，与转轴转动角度相关，
则工业机器人A和工业机器人B的两个连杆任意两点之间的距离为：
Figure 672489DEST_PATH_IMAGE083
令：
Figure 879610DEST_PATH_IMAGE085
先求得函数
Figure DEST_PATH_IMAGE086
取最小值，对函数求
Figure 962973DEST_PATH_IMAGE086
偏导，并令导数等于零：
Figure 24470DEST_PATH_IMAGE087
则，若上述方程组
Figure DEST_PATH_IMAGE088
有解，则工业机器人A和工业机器人B的连杆之间的最小距离为：
Figure DEST_PATH_IMAGE089
否则，工业机器人A和工业机器人B的连杆之间的最小距离为工业机器人A和工业机器人B的连杆端点之间距离的最小值。
CN202111233408.XA 2021-10-22 2021-10-22 一种适用于双机器人系统的防碰撞方法 Active CN113696187B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111233408.XA CN113696187B (zh) 2021-10-22 2021-10-22 一种适用于双机器人系统的防碰撞方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111233408.XA CN113696187B (zh) 2021-10-22 2021-10-22 一种适用于双机器人系统的防碰撞方法

Publications (2)

Publication Number Publication Date
CN113696187A CN113696187A (zh) 2021-11-26
CN113696187B true CN113696187B (zh) 2022-01-25

Family

ID=78647028

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111233408.XA Active CN113696187B (zh) 2021-10-22 2021-10-22 一种适用于双机器人系统的防碰撞方法

Country Status (1)

Country Link
CN (1) CN113696187B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114851207B (zh) * 2022-06-09 2023-08-25 珠海格力电器股份有限公司 一种双机器人的防碰撞控制方法、系统和多机器人系统
CN115716265B (zh) * 2022-10-31 2023-11-10 中国电器科学研究院股份有限公司 一种机器人双臂碰撞神经反射控制方法

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103895040B (zh) * 2014-04-10 2015-08-19 安凯 空间机械臂连杆间相互碰撞的检测方法
CN105856123A (zh) * 2015-01-21 2016-08-17 成都飞机工业（集团）有限责任公司 一种可用于机器人调型的随动定位装置
CN105241683B (zh) * 2015-11-11 2018-01-19 河北科技大学 一种双串联机械臂式移动操作机装置的碰撞检测方法
CN106166749B (zh) * 2016-06-29 2018-09-18 北京控制工程研究所 一种空间移动多臂机器人的移动轨迹规划方法
CN106945020B (zh) * 2017-05-18 2019-10-22 哈尔滨工业大学 一种空间双机械臂系统运动协调控制方法
US10145747B1 (en) * 2017-10-10 2018-12-04 Auris Health, Inc. Detection of undesirable forces on a surgical robotic arm
CN108638065B (zh) * 2018-05-15 2021-04-16 河南科技学院 一种排爆机器人双臂协同控制系统
EP3794314B1 (en) * 2018-06-25 2024-04-24 Intrinsic Innovation LLC Robot coordination in a shared workspace
US11668375B2 (en) * 2018-10-10 2023-06-06 Kyushu Institute Of Technology Parallel link mechanism and link actuation device
CN114502094A (zh) * 2019-09-26 2022-05-13 奥瑞斯健康公司 用于碰撞检测和避免的系统和方法
CN112264989B (zh) * 2020-06-17 2023-06-06 华中科技大学 一种基于邻域遍历的双机械臂协同避障方法
CN113334392B (zh) * 2021-08-06 2021-11-09 成都博恩思医学机器人有限公司 一种机械臂防碰撞方法、装置、机器人及存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Elbow Position Control of the Redundant Industrial;Taeyong Choi;《Elbow Position Control of the Redundant Industrial》;IEEE;20180326;第1997-2002页 *

Also Published As

Publication number Publication date
CN113696187A (zh) 2021-11-26

Similar Documents

Publication Publication Date Title
CN113696187B (zh) 一种适用于双机器人系统的防碰撞方法
CN107336230B (zh) 一种基于投影和距离判断的工业机器人碰撞预测方法
WO2018090323A1 (zh) 一种坐标系标定方法、系统及装置
Ragaglia et al. Safety-aware trajectory scaling for human-robot collaboration with prediction of human occupancy
CN111243017B (zh) 基于3d视觉的智能机器人抓取方法
US20100168919A1 (en) Control method and control system for manipulator
CN110888348B (zh) 一种基于激光slam的机器人码垛控制方法及其控制系统
CN110370271A (zh) 工业串联机器人的关节传动比误差校准方法
CN113715016A (zh) 一种基于3d视觉的机器人抓取方法、系统、装置及介质
CN105538015A (zh) 一种叶片类复杂薄壁型面零件自适应定位方法
CN114265364A (zh) 一种工业物联网的监测数据处理系统及方法
CN110815226B (zh) 一种机器人任意姿态任意位置下回初始位置的方法
CN106546184B (zh) 大型复杂曲面三维形貌机器人柔性测量系统
Han et al. Grasping control method of manipulator based on binocular vision combining target detection and trajectory planning
Chang et al. Implementation of an object-grasping robot arm using stereo vision measurement and fuzzy control
CN213259495U (zh) 一种确保速度和动量边界限制的工业机器人
CN116985134A (zh) 一种基于机器人图像处理的伺服控制方法与系统
Anggraeni et al. Design and development of multiple mobile manipulator robots using gazebo-ros
CN114074323B (zh) 一种确保机器人速度和动量边界限制的安全系统
Papoutsidakis et al. Intelligent design and algorithms to control a stereoscopic camera on a robotic workspace
CN112077841B (zh) 一种提升机器人手臂操纵精度的多关节联动方法及系统
CN108107882A (zh) 基于光学运动跟踪的服务机器人自动标定与检测系统
CN114074324B (zh) 一种新颖的确保机器人速度和动量边界限制的安全系统
CN111127638A (zh) 利用三维模板库实现工件突出标志位定位抓取点方法
Ma et al. A geometric algorithm for symmetric workpiece localization

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