CN118789552B - 基于六轴机械手的自动化设备控制方法及系统 - Google Patents

基于六轴机械手的自动化设备控制方法及系统 Download PDF

Info

Publication number
CN118789552B
CN118789552B CN202411134713.7A CN202411134713A CN118789552B CN 118789552 B CN118789552 B CN 118789552B CN 202411134713 A CN202411134713 A CN 202411134713A CN 118789552 B CN118789552 B CN 118789552B
Authority
CN
China
Prior art keywords
interval
target
posture
angle
link
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
CN202411134713.7A
Other languages
English (en)
Other versions
CN118789552A (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.)
Shenzhen Yiling Automation Equipment Co ltd
Original Assignee
Shenzhen Yiling Automation Equipment 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 Shenzhen Yiling Automation Equipment Co ltd filed Critical Shenzhen Yiling Automation Equipment Co ltd
Priority to CN202411134713.7A priority Critical patent/CN118789552B/zh
Publication of CN118789552A publication Critical patent/CN118789552A/zh
Application granted granted Critical
Publication of CN118789552B publication Critical patent/CN118789552B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/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
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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
    • G05B11/00—Automatic controllers
    • G05B11/01—Automatic controllers electric
    • G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
    • G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P. I., P. I. D.
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

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

Abstract

本发明涉及机械手技术领域，揭露一种基于六轴机械手的自动化设备控制方法及系统，方法包括：利用末端坐标点规划六轴机械手的目标位姿，计算当前位姿至目标位姿的位姿成本；构建六轴机械手的传递函数，利用位姿误差构建六轴机械手的PID控制器，计算PID控制器的增益参数；基于增益参数与位姿误差，利用PID控制器输出六轴机械手的规划控制信号；计算从当前控制信号至规划控制信号的信号控制风险，根据信号控制风险，从规划控制信号中筛选设备控制信号，利用设备控制信号对六轴机械手进行设备控制，得到六轴机械手的设备控制结果。本发明可以明确末端前面的机械臂与关节的运动动作与运动轨迹，并代替模糊PID控制的方式来对六轴机械手进行控制。

Description

基于六轴机械手的自动化设备控制方法及系统
技术领域
本发明涉及机械手技术领域，尤其涉及一种基于六轴机械手的自动化设备控制方法及系统。
背景技术
六轴机械手是指具备六个自由度的机械手，这六个自由度分别是x轴自由度、y轴自由度、z轴自由度、绕x轴旋转的自由度、绕y轴旋转的自由度、绕z轴旋转的自由度，基于六轴机械手的自动化设备控制是指对六轴机械手的运动动作与运动轨迹进行控制的过程。
目前，对六轴机械手进行控制之前需要先确定六轴机械手下一步要挪动的位置，一般是六轴机械手的末端要接触到货物，从而夹取货物，现有技术一般仅控制六轴机械手的末端的运动动作与运动轨迹，而六轴机械手的末端的控制又依靠末端前面的机械臂与关节来实现，但现有技术并未明确末端前面的机械臂与关节的运动动作与运动轨迹，其次，从机械手的当前动作转换成其他位置下的动作需要缓慢分间隔进行，并不是一次性动作，而是分化成需要小动作来完成当前动作转换成其他位置下的动作，在这个过程中还需要考虑机械手是否保持稳定性，即防止机械手受外界干扰而产生抖动，现有技术常通过模糊PID的方式来保持机械手的稳定性，模糊PID控制的方式需要进行控制参数的调整、模糊集选择及大量数据计算，而参数的调整需要一定的经验和专业知识，需要由人工专家来决策选取，大量数据计算需要较大的计算资源，模糊集选择对控制效果有较大影响，如果选择不当，可能会导致控制效果下降或不稳定。因此，亟待一种解决方案可以明确末端前面的机械臂与关节的运动动作与运动轨迹，并代替模糊PID控制的方式来对六轴机械手进行控制。
发明内容
为了解决上述问题，本发明提供了一种基于六轴机械手的自动化设备控制方法及系统，可以明确末端前面的机械臂与关节的运动动作与运动轨迹，并代替模糊PID控制的方式来对六轴机械手进行控制。
第一方面，本发明提供了一种基于六轴机械手的自动化设备控制方法，包括：
确定六轴机械手的末端的末端坐标点，采集所述六轴机械手的当前位姿，其中，所述六轴机械手包括末端、第一连杆、第二连杆、第一连杆起点、第一连杆终点，所述当前位姿包括当前坐标点与当前角度值；
利用所述末端坐标点规划所述六轴机械手的目标位姿，其中，所述目标位姿包括目标坐标点与目标角度值，基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，利用所述位姿成本从所述目标位姿中选取最终位姿；
计算所述当前位姿与所述最终位姿之间的位姿误差，构建所述六轴机械手的传递函数，利用所述位姿误差构建所述六轴机械手的PID控制器，基于所述传递函数，计算所述PID控制器的增益参数；
基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，并采集所述六轴机械手的当前控制信号；
计算从所述当前控制信号至所述规划控制信号的信号控制风险，根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，利用所述设备控制信号对所述六轴机械手进行设备控制，得到所述六轴机械手的设备控制结果。
在第一方面的一种可能实现方式中，所述利用所述末端坐标点规划所述六轴机械手的目标位姿，包括：
获取所述六轴机械手的旋转角度范围与连杆长度；
基于所述旋转角度范围与所述连杆长度，利用下述公式构建所述末端坐标点的目标函数：
lx＝l1cosθx
ly＝l1cosθy
lz＝l1cosθz
l′x＝l2cosθ′x
l′y＝l2cosθ′y
l′z＝l2cosθ′z
l″x＝lx+l′x
l″y＝ly+l′y
l″z＝lz+l′z
l″x＝l1cosθx+l2cosθ′x
l″y＝l1cosθy+l2cosθ′y
l″z＝l1cosθz+l2cosθ′z
其中，F表示目标函数，F包括min|l″x-l″′x|、min|l″y-l″′y|及min|l″z-l″′z|，(l″′x,l″′y,l″′z)表示所述末端坐标点，(l″x,l″y,l″z)表示所述末端坐标点的待优化坐标点，l1表示所述连杆长度中第一连杆的长度，l2表示所述连杆长度中第二连杆的长度，(θx,θy,θz)表示以第一连杆起点为原点的直角坐标系中第一连杆相对于直角坐标系的偏转角度，(θ′x,θ′y,θ′z)表示以第一连杆终点为原点的直角坐标系中第二连杆相对于直角坐标系的偏转角度，lx表示以第一连杆起点为原点的直角坐标系中第一连杆在x轴上的映射长度，ly表示以第一连杆起点为原点的直角坐标系中第一连杆在y轴上的映射长度，lz表示以第一连杆起点为原点的直角坐标系中第一连杆在z轴上的映射长度，l′x表示以第一连杆终点为原点的直角坐标系中第二连杆在x轴上的映射长度，l′y表示以第一连杆终点为原点的直角坐标系中第二连杆在y轴上的映射长度，l′z表示以第一连杆终点为原点的直角坐标系中第二连杆在z轴上的映射长度，θx,θy,θz,θ′x,θ'y,θ'z属于所述旋转角度范围；
利用下述公式对所述目标函数中的角度参数进行角度更新，得到更新角度：
v′ij＝vij+c1×rand×(pbestij-xij)+c2×rrand×(gbestij-xij)
x′ij＝xij+vij
其中，x′ij表示更新角度，v′ij表示对原始更新速度进行更新之后的速度，i表示θ的序号，j表示θ'的序号，vij表示θ'的原始更新速度，c1与c2表示学习因子，c1＝c2＝2，rand表示介于0～1之间的随机数，xij表示θ'的当前数值，pbestij表示θ'的邻近历史取值中F值为0的邻近历史取值，gbestij表示θ'的全部历史取值中F值为0的历史取值，θ'包括θ'x,θ'y,θ'z，θ包括θx,θy,θz；
判断所述更新角度在所述目标函数中对应的目标函数值是否为预设阈值；
当所述更新角度在所述目标函数中对应的目标函数值为所述预设阈值时，利用所述更新角度计算所述六轴机械手的目标坐标点与目标角度值；
将所述目标坐标点与所述目标角度值作为所述目标位姿。
在第一方面的一种可能实现方式中，所述基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，包括：
获取所述目标位姿中的目标坐标点与目标角度值；
利用下述公式计算所述当前坐标点至所述目标坐标点的坐标成本：
U＝u1+u2
其中，U表示坐标成本，u1表示当前坐标点中第一连杆终点的坐标与目标坐标点中第一连杆终点的坐标之间的差值的绝对值，u2当前坐标点中末端的坐标与末端坐标点之间的差值的绝对值；
利用下述公式计算所述当前角度值至所述目标坐标点的角度成本：
V＝v1+v2
其中，V表示角度成本，v1表示当前角度值中第一连杆终点的角度与目标角度值中第一连杆终点的角度之间的差值的绝对值，v2当前角度值中末端的角度与末端角度值之间的差值的绝对值；
将所述坐标成本与所述角度成本之和作为所述当前位姿至所述目标位姿的位姿成本。
在第一方面的一种可能实现方式中，所述构建所述六轴机械手的传递函数，包括：
利用下述公式构建所述六轴机械手的传递函数：
p(s)*(s2+αs+β)＝γ*q(s)
s2p(s)+αsp(s)+βp(s)＝γq(s)
其中，表示所述六轴机械手的传递函数，t表示时域中的时间自变量，p(t)表示在PID控制器的控制下六轴机械手被调整之后的位姿，q(t)表示PID控制器向六轴机械手输入的位姿控制信号，α、β、γ表示常数参数，s表示对p(t)进行拉普拉斯变换时将t进行变换而得到的自变量，p(s)表示p(t)的拉普拉斯变换形式，q(s)表示q(t)的拉普拉斯变换形式。
在第一方面的一种可能实现方式中，所述基于所述传递函数，计算所述PID控制器的增益参数，包括：
获取PID控制器对应的输出误差；
基于所述输出误差与所述传递函数，构建所述六轴机械手的状态方程矩阵；
构建所述PID控制器下的李雅普诺夫原模型；
对所述李雅普诺夫原模型进行模型求导，得到李雅普诺夫原模型的导数；
计算输出误差下所述PID控制器的反函数；
将所述状态方程矩阵与所述反函数代入所述李雅普诺夫原模型的导数中，得到李雅普诺夫导数模型；
判断是否可以构建所述李雅普诺夫原模型的正定矩阵；
在可以构建所述李雅普诺夫原模型的正定矩阵时，判定所述李雅普诺夫原模型大于预设阈值；
在所述李雅普诺夫原模型大于预设阈值时，构建所述李雅普诺夫导数模型的负定矩阵；
在所述PID控制器的比例增益、积分增益及微分增益符合所述负定矩阵时，确定所述PID控制器的增益参数。
在第一方面的一种可能实现方式中，所述基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，包括：
利用所述增益参数调整所述PID控制器，得到调整控制器；
将所述位姿误差代入所述调整控制器中；
在所述调整控制器中输出所述位姿误差对应的规划控制信号。
在第一方面的一种可能实现方式中，所述计算从所述当前控制信号至所述规划控制信号的信号控制风险，包括：
查询所述当前控制信号与所述规划控制信号中每两个相邻控制信号之间的信号控制区间；
按照每个信号控制区间之间的相邻关系，随机合并所述信号控制区间，得到合并控制区间；
识别历史时段中是否存在不小于所述合并控制区间的区间；
在历史时段中存在不小于所述合并控制区间的区间时，将无风险作为所述信号控制风险；
在历史时段中不存在不小于所述合并控制区间的区间时，计算所述合并控制区间的区间长度；
识别所述历史时段中是否存在与所述区间长度一致的目标长度；
查询所述区间长度下的区间规模与所述目标长度下的目标规模；
计算所述区间规模与所述目标规模之间的规模标准差；
利用所述规模标准差构建所述区间规模的标准差阈值；
在所述区间规模大于所述标准差阈值时，将有风险作为所述信号控制风险；
在所述区间规模不大于所述标准差阈值时，将无风险作为所述信号控制风险。
在第一方面的一种可能实现方式中，所述计算所述区间规模与所述目标规模之间的规模标准差，包括：
获取所述区间规模中的区间角度差与区间坐标距离；
计算所述区间角度差与所述区间坐标距离之间的第一角度-坐标和；
获取所述目标规模中的目标角度差与目标坐标距离；
计算所述目标角度差与所述目标坐标距离之间的第二角度-坐标和；
计算所述第一角度-坐标和与所述第二角度-坐标和之间的和均值；
根据所述第一角度-坐标和、所述第二角度-坐标和及所述和均值，计算所述区间规模与所述目标规模之间的规模标准差。
在第一方面的一种可能实现方式中，所述根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，包括：
在所述信号控制风险为无风险时，保留所述无风险对应的合并控制区间，得到保留区间；
查询所述保留区间中每个相邻的保留区间之间是否区间连续；
在所述保留区间中每个相邻的保留区间之间未区间连续时，根据所述保留区间中的信号控制区间，缩短所述保留区间的区间长度，得到缩短区间，以通过所述缩短区间对齐所述保留区间中每个相邻的保留区间，得到对齐区间；
计算所述对齐区间的区间数目；
从所述区间数目中选取最小数目；
从所述对齐区间中获取所述最小数目对应的目标对齐区间；
将所述目标对齐区间的边界上的规划控制信号作为所述设备控制信号。
第二方面，本发明提供了一种基于六轴机械手的自动化设备控制系统，所述系统包括：
位姿采集模块，用于确定六轴机械手的末端的末端坐标点，采集所述六轴机械手的当前位姿，其中，所述六轴机械手包括末端、第一连杆、第二连杆、第一连杆起点、第一连杆终点，所述当前位姿包括当前坐标点与当前角度值；
位姿选取模块，用于利用所述末端坐标点规划所述六轴机械手的目标位姿，其中，所述目标位姿包括目标坐标点与目标角度值，基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，利用所述位姿成本从所述目标位姿中选取最终位姿；
参数计算模块，用于计算所述当前位姿与所述最终位姿之间的位姿误差，构建所述六轴机械手的传递函数，利用所述位姿误差构建所述六轴机械手的PID控制器，基于所述传递函数，计算所述PID控制器的增益参数；
信号采集模块，用于基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，并采集所述六轴机械手的当前控制信号；
设备控制模块，用于计算从所述当前控制信号至所述规划控制信号的信号控制风险，根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，利用所述设备控制信号对所述六轴机械手进行设备控制，得到所述六轴机械手的设备控制结果。
与现有技术相比，本方案的技术原理及有益效果在于：
本发明实施例通过利用所述末端坐标点规划所述六轴机械手的目标位姿，以用于明确末端前面的第一连杆、第二连杆及第一连杆终点的运动动作与运动轨迹，进一步地，本发明实施例通过基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，以用于选取位姿成本最小的目标位姿，进一步地，本发明实施例通过构建所述六轴机械手的传递函数，以用于构建PID控制器的输出控制信号与六轴机械手输出的位姿之间的传递关系，进一步地，本发明实施例通过基于所述传递函数，计算所述PID控制器的增益参数，以用于利用李雅普诺夫模型代替模糊PID控制的方式来对六轴机械手进行稳定性控制，本发明实施例通过计算从所述当前控制信号至所述规划控制信号的信号控制风险，以用于在PID控制器输出一序列的规划控制信号的前提下，计算每个当前的控制信号至下一个控制信号的后面的控制信号的过程中是否会对六轴机械手造成机械障碍，进一步地，本发明实施例通过根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，以用于将原本PID控制器输出的连续的规划控制信号中无风险的规划控制信号扩大，这样可以减少频繁的调整控制信号的次数。因此，本发明实施例提出的一种基于六轴机械手的自动化设备控制方法及系统，可以明确末端前面的机械臂与关节的运动动作与运动轨迹，并代替模糊PID控制的方式来对六轴机械手进行控制。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一实施例提供的一种基于六轴机械手的自动化设备控制方法的流程示意图；
图2为本发明一实施例提供的一种基于六轴机械手的自动化设备控制方法的货物空间的示意图；
图3为本发明一实施例提供的一种基于六轴机械手的自动化设备控制系统的模块示意图。
具体实施方式
应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不用于限定本发明。
本发明实施例提供一种基于六轴机械手的自动化设备控制方法，所述基于六轴机械手的自动化设备控制方法的执行主体包括但不限于服务端、终端等能够被配置为执行本发明实施例提供的该方法的电子设备中的至少一种。换言之，所述基于六轴机械手的自动化设备控制方法可以由安装在终端设备或服务端设备的软件或硬件来执行，所述软件可以是区块链平台。所述服务端包括但不限于：单台服务器、服务器集群、云端服务器或云端服务器集群等。所述服务器可以是独立的服务器，也可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、内容分发网络(ContentDeliveryNetwork，CDN)、以及大数据和人工智能平台等基础云计算服务的云服务器。
参阅图1所示，是本发明一实施例提供的基于六轴机械手的自动化设备控制方法的流程示意图。其中，图1中描述的基于六轴机械手的自动化设备控制方法包括：
S1、确定六轴机械手的末端的末端坐标点，采集所述六轴机械手的当前位姿，其中，所述六轴机械手包括末端、第一连杆、第二连杆、第一连杆起点、第一连杆终点，所述当前位姿包括当前坐标点与当前角度值。
本发明实施例中，所述六轴机械手是指具备六个自由度的机械手，这六个自由度分别是x轴自由度、y轴自由度、z轴自由度、绕x轴旋转的自由度、绕y轴旋转的自由度、绕z轴旋转的自由度，所述末端坐标点是指末端在下一步需要达到的坐标位置，末端当前并未在所述末端坐标点处。
参阅图2所示，为本发明一实施例提供的一种基于六轴机械手的自动化设备控制方法的六轴机械手的示意图。在图2中，1表示末端，5表示第一连杆，4表示第二连杆，3表示一连杆起点，2表示第一连杆终点。
需要说明的是，当前坐标点包括第一连杆起点的坐标、第一连杆终点的坐标及末端的坐标，所述当前角度值第一连杆的角度值与第二连杆的角度值。
S2、利用所述末端坐标点规划所述六轴机械手的目标位姿，其中，所述目标位姿包括目标坐标点与目标角度值，基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，利用所述位姿成本从所述目标位姿中选取最终位姿。
本发明实施例通过利用所述末端坐标点规划所述六轴机械手的目标位姿，以用于明确末端前面的第一连杆、第二连杆及第一连杆终点的运动动作与运动轨迹。
其中，所述目标位姿是指所述六轴机械手在下一步需要达到的坐标位置与旋转角度，所述六轴机械手当前并未在所述目标位姿处。
本发明的一实施例中，所述利用所述末端坐标点规划所述六轴机械手的目标位姿，包括：获取所述六轴机械手的旋转角度范围与连杆长度；基于所述旋转角度范围与所述连杆长度，利用下述公式构建所述末端坐标点的目标函数：
lx＝l1cosθx
ly＝l1cosθy
lz＝l1cosθz
l′x＝l2cosθ′x
l′y＝l2cosθ′y
l′Z＝l2cosθ′z
l″x＝lx+l′x
l″y＝ly+l′y
l″ z＝lz+l′z
l″x＝l1cosθx+l2cosθ′x
l″y＝l1cosθy+l2cosθ′y
l″z＝l1cosθz+l2cosθ′z
其中，F表示目标函数，F包括min|l”x-l”'x|、min|l”y-l”'y|及min|l”z-l”'z|，(l”'x,l”'y,l”'z)表示所述末端坐标点，(l”x,l”y,l”z)表示所述末端坐标点的待优化坐标点，l1表示所述连杆长度中第一连杆的长度，l2表示所述连杆长度中第二连杆的长度，(θx,θy,θz)表示以第一连杆起点为原点的直角坐标系中第一连杆相对于直角坐标系的偏转角度，(θ'x,θ'y,θ'z)表示以第一连杆终点为原点的直角坐标系中第二连杆相对于直角坐标系的偏转角度，lx表示以第一连杆起点为原点的直角坐标系中第一连杆在x轴上的映射长度，ly表示以第一连杆起点为原点的直角坐标系中第一连杆在y轴上的映射长度，lz表示以第一连杆起点为原点的直角坐标系中第一连杆在z轴上的映射长度，l'x表示以第一连杆终点为原点的直角坐标系中第二连杆在x轴上的映射长度，l'y表示以第一连杆终点为原点的直角坐标系中第二连杆在y轴上的映射长度，l'z表示以第一连杆终点为原点的直角坐标系中第二连杆在z轴上的映射长度，θx,θy,θz,θ'x,θ'y,θ'z属于所述旋转角度范围；
利用下述公式对所述目标函数中的角度参数进行角度更新，得到更新角度：
v′ij＝vij+c1×rand×(pbestij-xij)+c2×rand×(gbestij-xij)
x′ij＝xij+vij
其中，x'ij表示更新角度，v'ij表示对原始更新速度进行更新之后的速度，i表示θ的序号，j表示θ'的序号，vij表示θ'的原始更新速度，c1与c2表示学习因子，c1＝c2＝2，rand表示介于0～1之间的随机数，xij表示θ'的当前数值，pbestij表示θ'的邻近历史取值中F值为0的邻近历史取值，gbestij表示θ'的全部历史取值中F值为0的历史取值，θ'包括θ'x,θ'y,θ'z，θ包括θx,θy,θz；
判断所述更新角度在所述目标函数中对应的目标函数值是否为预设阈值；当所述更新角度在所述目标函数中对应的目标函数值为所述预设阈值时，利用所述更新角度计算所述六轴机械手的目标坐标点与目标角度值；将所述目标坐标点与所述目标角度值作为所述目标位姿。
其中，所述旋转角度范围是指所述六轴机械手的第一连杆与第二连杆可以旋转的最大角度区间，所述连杆长度包括第一连杆的长度与第二连杆的长度，需要说明的是，上述(l”x,l”y,l”z)表示所述末端坐标点的待优化坐标点中，(l”x,l”y,l”z)与末端坐标点的含义一致，(l”x,l”y,l”z)为需要经过目标函数优化的数值，邻近历史取值是指历史取值中与θ'邻近的其他θ'取值，所述预设阈值的取值为0，表示(l”x,l”y,l”z)的取值与末端坐标点的取值一致，所述更新角度是指θ下的θ'的取值，因此通过更新角度可以更新得到θ与θ'，而通过θ、θ'及连杆长度可以计算得到目标坐标点与目标角度值，具体计算方式与上述计算lx、ly、lz的计算原理类似，在此不做进一步地赘述。
进一步地，本发明实施例通过基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，以用于选取位姿成本最小的目标位姿。
本发明的一实施例中，所述基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，包括：获取所述目标位姿中的目标坐标点与目标角度值；利用下述公式计算所述当前坐标点至所述目标坐标点的坐标成本：
U＝u1+u2
其中，U表示坐标成本，u1表示当前坐标点中第一连杆终点的坐标与目标坐标点中第一连杆终点的坐标之间的差值的绝对值，u2当前坐标点中末端的坐标与末端坐标点之间的差值的绝对值；
利用下述公式计算所述当前角度值至所述目标坐标点的角度成本：
V＝v1+v2
其中，V表示角度成本，v1表示当前角度值中第一连杆终点的角度与目标角度值中第一连杆终点的角度之间的差值的绝对值，v2当前角度值中末端的角度与末端角度值之间的差值的绝对值；
将所述坐标成本与所述角度成本之和作为所述当前位姿至所述目标位姿的位姿成本。
可选地，所述利用所述位姿成本从所述目标位姿中选取最终位姿的过程是指选取位姿成本最小的目标位姿。
S3、计算所述当前位姿与所述最终位姿之间的位姿误差，构建所述六轴机械手的传递函数，利用所述位姿误差构建所述六轴机械手的PID控制器，基于所述传递函数，计算所述PID控制器的增益参数。
本发明实施例中，所述位姿误差是指所述当前位姿与所述最终位姿之间的位姿差值，需要说明的，所述计算所述当前位姿与所述最终位姿之间的位姿误差的原理与上述基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本的原理类似，在此不做进一步地赘述。
进一步地，本发明实施例通过构建所述六轴机械手的传递函数，以用于构建PID控制器的输出控制信号与六轴机械手输出的位姿之间的传递关系。
本发明的一实施例中，所述构建所述六轴机械手的传递函数，包括：利用下述公式构建所述六轴机械手的传递函数：
p(s)*(s2+αs+β)＝γ*q(s)
s2p(s)+αsp(s)+βp(s)＝γq(s)
其中，表示所述六轴机械手的传递函数，t表示时域中的时间自变量，p(t)表示在PID控制器的控制下六轴机械手被调整之后的位姿，q(t)表示PID控制器向六轴机械手输入的位姿控制信号，α、β、γ表示常数参数，s表示对p(t)进行拉普拉斯变换时将t进行变换而得到的自变量，p(s)表示p(t)的拉普拉斯变换形式，q(s)表示q(t)的拉普拉斯变换形式。
可选地，所述利用所述位姿误差构建所述六轴机械手的PID控制器的过程为：基于所述位姿误差，利用下述公式构建所述六轴机械手的PID控制器：
其中，q(t)表示PID控制器向六轴机械手输入的位姿控制信号，k1表示比例增益，k2表示积分增益，k3表示微分增益，e(t)表示p(t)与最终位姿之间的输出误差，e0(t)表示当前位姿与所述最终位姿之间的位姿误差，q0(t)表示在位姿误差下PID控制器向六轴机械手输入的位姿控制信号，t表示时域中的时间自变量。
需要说明的是，将e0(t)输入PID控制器之后，PID控制器基于e0(t)来输出q0(t)，q0(t)作用于六轴机械手，六轴机械手在q0(t)的作用下输出p(t)，而在p(t)与最终位姿之间的位姿误差e(t)控制下PID控制器输出q(t)。
进一步地，本发明实施例通过基于所述传递函数，计算所述PID控制器的增益参数，以用于利用李雅普诺夫模型代替模糊PID控制的方式来对六轴机械手进行稳定性控制。
其中，所述增益参数是指PID控制器中比例增益、积分增益及微分增益优化后的数值。
本发明的一实施例中，所述基于所述传递函数，计算所述PID控制器的增益参数，包括：获取PID控制器对应的输出误差；基于所述输出误差与所述传递函数，利用下述公式构建所述六轴机械手的状态方程矩阵：
其中，表示状态方程矩阵，en(t)表示e(t)的n阶导数，pn(t)表示p(t)的n阶导数，e(t)表示输出误差，p(t)表示在PID控制器的控制下六轴机械手被调整之后的位姿，t表示时域中的时间自变量，表示所述六轴机械手的传递函数的变形公式，δ'与ε'表示将代入 中的q(t)之后计算得到的已知参数，q(t)表示PID控制器向六轴机械手输入的位姿控制信号，δ与ε表示未知参数；
利用下述公式构建所述PID控制器下的李雅普诺夫原模型：
I(en(t))＝(en(t))TH(en(t))
其中，I(en(t))表示李雅普诺夫原模型，en(t)表示e(t)的n阶导数，H表示用于判别李雅普诺夫原模型与李雅普诺夫导数模型的稳定性的参数；
对所述李雅普诺夫原模型进行模型求导，得到李雅普诺夫原模型的导数；计算输出误差下所述PID控制器的反函数；将所述状态方程矩阵与所述反函数代入所述李雅普诺夫原模型的导数中，得到李雅普诺夫导数模型；判断是否可以构建所述李雅普诺夫原模型的正定矩阵；在可以构建所述李雅普诺夫原模型的正定矩阵时，判定所述李雅普诺夫原模型大于预设阈值；在所述李雅普诺夫原模型大于预设阈值时，构建所述李雅普诺夫导数模型的负定矩阵；在所述PID控制器的比例增益、积分增益及微分增益符合所述负定矩阵时，确定所述PID控制器的增益参数。
其中，所述PID控制器的控制下六轴机械手被调整之后的位姿是指p(t)，所述输出误差下所述PID控制器的反函数由q(t)＝f(en(t))推导至en(t)＝
f-1(q(t))得到，所述预设阈值取值为0。
可选地，所述将所述状态方程矩阵与所述反函数代入所述李雅普诺夫原模型的导数中，得到李雅普诺夫导数模型的公式为：
其中，表示李雅普诺夫导数模型，en(t)表示e(t)的n阶导数，δ'与ε'表示将代入中的q(t)之后计算得到的已知参数，H表示用于判别李雅普诺夫原模型与李雅普诺夫导数模型的稳定性的矩阵，表示输出误差导数模型，表示状态方程矩阵，f(en(t))表示自变量为en(t)的
可选地，所述构建所述李雅普诺夫导数模型的负定矩阵的公式为：
其中，Q(H)表示李雅普诺夫导数模型的负定矩阵，表示李雅普诺夫导数模型，H表示用于判别李雅普诺夫原模型与李雅普诺夫导数模型的稳定性的矩阵。
需要说明的是，Q(H)由李雅普诺夫导数模型中的H、k1、k2、k3、δ'及ε'变形而来。
可选地，所述判断是否可以构建所述李雅普诺夫原模型的正定矩阵的过程是指判断是否存在正定的H矩阵的过程，在这个过程中，需要选取参数代入H矩阵中，选取参数的过程于上述计算更新角度的原理类似，在此不做进一步地赘述，所述在所述PID控制器的比例增益、积分增益及微分增益符合所述负定矩阵时，确定所述PID控制器的增益参数的原理与上述构建所述李雅普诺夫原模型的正定矩阵的原理类似，在此不做进一步地赘述。
S4、基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，并采集所述六轴机械手的当前控制信号。
本发明的一实施例中，所述基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，包括：利用所述增益参数调整所述PID控制器，得到调整控制器；将所述位姿误差代入所述调整控制器中；在所述调整控制器中输出所述位姿误差对应的规划控制信号。
可选地，所述利用所述增益参数调整所述PID控制器，得到调整控制器的过程是指将PID控制器中的比例增益、积分增益及微分增益调整为所述增益参数。
S5、计算从所述当前控制信号至所述规划控制信号的信号控制风险，根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，利用所述设备控制信号对所述六轴机械手进行设备控制，得到所述六轴机械手的设备控制结果。
本发明实施例通过计算从所述当前控制信号至所述规划控制信号的信号控制风险，以用于在PID控制器输出一序列的规划控制信号的前提下，计算每个当前的控制信号至下一个控制信号的后面的控制信号的过程中是否会对六轴机械手造成机械障碍。
其中，所述信号控制风险是指信号控制时会对六轴机械手造成的机械障碍的风险。
本发明的一实施例中，所述计算从所述当前控制信号至所述规划控制信号的信号控制风险，包括：查询所述当前控制信号与所述规划控制信号中每两个相邻控制信号之间的信号控制区间；按照每个信号控制区间之间的相邻关系，随机合并所述信号控制区间，得到合并控制区间；识别历史时段中是否存在不小于所述合并控制区间的区间；在历史时段中存在不小于所述合并控制区间的区间时，将无风险作为所述信号控制风险；在历史时段中不存在不小于所述合并控制区间的区间时，计算所述合并控制区间的区间长度；识别所述历史时段中是否存在与所述区间长度一致的目标长度；查询所述区间长度下的区间规模与所述目标长度下的目标规模；计算所述区间规模与所述目标规模之间的规模标准差；利用所述规模标准差构建所述区间规模的标准差阈值；在所述区间规模大于所述标准差阈值时，将有风险作为所述信号控制风险；在所述区间规模不大于所述标准差阈值时，将无风险作为所述信号控制风险。
可选地，所述按照每个信号控制区间之间的相邻关系，随机合并所述信号控制区间，得到合并控制区间的过程为：例如所述当前控制信号与所述规划控制信号中每个控制信号为Dt，Dt表示第t时刻的控制信号，信号控制区间为Dt-2～Dt-1、Dt-1～Dt、Dt～Dt+1、Dt+1～Dt+2，合并控制区间为Dt-2～Dt、Dt-1～Dt+2，也就是将多个单独的信号控制区间合并为一个合并控制区间，合并的多个单独的信号控制区间需要为相邻或连续的关系，且合并的数目随机，并不固定，所述区间规模是指从这个区间长度的起点的信号调整为这个区间长度的终点的信号的过程中六轴机械手在角度与坐标上的变化的量值，所述目标规模的含义与所述区间规模的含义类似。
本发明的又一实施例中，所述计算所述区间规模与所述目标规模之间的规模标准差，包括：获取所述区间规模中的区间角度差与区间坐标距离；计算所述区间角度差与所述区间坐标距离之间的第一角度-坐标和；获取所述目标规模中的目标角度差与目标坐标距离；计算所述目标角度差与所述目标坐标距离之间的第二角度-坐标和；计算所述第一角度-坐标和与所述第二角度-坐标和之间的和均值；根据所述第一角度-坐标和、所述第二角度-坐标和及所述和均值，计算所述区间规模与所述目标规模之间的规模标准差。
可选地，所述利用所述规模标准差构建所述区间规模的标准差阈值的过程是指按需利用某个常数对规模标准差进行缩小或放大，从而得到标准差阈值。
进一步地，本发明实施例通过根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，以用于将原本PID控制器输出的连续的规划控制信号中无风险的规划控制信号扩大，这样可以减少频繁的调整控制信号的次数。
本发明的一实施例中，所述根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，包括：在所述信号控制风险为无风险时，保留所述无风险对应的合并控制区间，得到保留区间；查询所述保留区间中每个相邻的保留区间之间是否区间连续；在所述保留区间中每个相邻的保留区间之间未区间连续时，根据所述保留区间中的信号控制区间，缩短所述保留区间的区间长度，得到缩短区间，以通过所述缩短区间对齐所述保留区间中每个相邻的保留区间，得到对齐区间；计算所述对齐区间的区间数目；从所述区间数目中选取最小数目；从所述对齐区间中获取所述最小数目对应的目标对齐区间；将所述目标对齐区间的边界上的规划控制信号作为所述设备控制信号。
其中，所述对齐区间是指边界连续的多个保留区间与缩短区间。
示例性地，根据所述保留区间中的信号控制区间，缩短所述保留区间的区间长度，得到缩短区间的过程是指：在保留区间为Dt-2～Dt、Dt-1～Dt+2时，缩短Dt-2～Dt为Dt-2～Dt-1，这样Dt-2～Dt-1与Dt-2～Dt-1之间连续(这两个区间的边界对齐，为对齐区间)，或者是，缩短Dt-1～Dt+2为Dt～Dt+2，这样Dt-2～Dt与Dt～Dt+2之间连续(这两个区间的边界对齐，为对齐区间)，由此可知，对齐区间并不同一，可以有多种，而后续的从所述对齐区间中获取所述最小数目对应的目标对齐区间的目的就在于选取最小次数，减少信号调整次数，因为对齐区间的每个边界上都是一个控制信号的数值，若对齐区间里面的区间数目较多，则信号调整次数也会变多。
可以看出，本发明实施例通过利用所述末端坐标点规划所述六轴机械手的目标位姿，以用于明确末端前面的第一连杆、第二连杆及第一连杆终点的运动动作与运动轨迹，进一步地，本发明实施例通过基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，以用于选取位姿成本最小的目标位姿，进一步地，本发明实施例通过构建所述六轴机械手的传递函数，以用于构建PID控制器的输出控制信号与六轴机械手输出的位姿之间的传递关系，进一步地，本发明实施例通过基于所述传递函数，计算所述PID控制器的增益参数，以用于利用李雅普诺夫模型代替模糊PID控制的方式来对六轴机械手进行稳定性控制，本发明实施例通过计算从所述当前控制信号至所述规划控制信号的信号控制风险，以用于在PID控制器输出一序列的规划控制信号的前提下，计算每个当前的控制信号至下一个控制信号的后面的控制信号的过程中是否会对六轴机械手造成机械障碍，进一步地，本发明实施例通过根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，以用于将原本PID控制器输出的连续的规划控制信号中无风险的规划控制信号扩大，这样可以减少频繁的调整控制信号的次数。因此，本发明实施例提出的一种基于六轴机械手的自动化设备控制方法及系统，可以明确末端前面的机械臂与关节的运动动作与运动轨迹，并代替模糊PID控制的方式来对六轴机械手进行控制。
如图3所示，是本发明基于六轴机械手的自动化设备控制系统功能模块图。
本发明所述基于六轴机械手的自动化设备控制系统300可以安装于电子设备中。根据实现的功能，所述基于六轴机械手的自动化设备控制系统可以包括位姿采集模块301、位姿选取模块302、参数计算模块303、信号采集模块304及设备控制模块305。本发明所述模块也可以称之为单元，是指一种能够被电子设备处理器所执行，并且能够完成固定功能的一系列计算机程序段，其存储在电子设备的存储器中。
在本发明实施例中，关于各模块/单元的功能如下：
所述位姿采集模块301，用于确定六轴机械手的末端的末端坐标点，采集所述六轴机械手的当前位姿，其中，所述六轴机械手包括末端、第一连杆、第二连杆、第一连杆起点、第一连杆终点，所述当前位姿包括当前坐标点与当前角度值；
所述位姿选取模块302，用于利用所述末端坐标点规划所述六轴机械手的目标位姿，其中，所述目标位姿包括目标坐标点与目标角度值，基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，利用所述位姿成本从所述目标位姿中选取最终位姿；
所述参数计算模块303，用于计算所述当前位姿与所述最终位姿之间的位姿误差，构建所述六轴机械手的传递函数，利用所述位姿误差构建所述六轴机械手的PID控制器，基于所述传递函数，计算所述PID控制器的增益参数；
所述信号采集模块304，用于基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，并采集所述六轴机械手的当前控制信号；
所述设备控制模块305，用于计算从所述当前控制信号至所述规划控制信号的信号控制风险，根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，利用所述设备控制信号对所述六轴机械手进行设备控制，得到所述六轴机械手的设备控制结果。
详细地，本发明实施例中所述基于六轴机械手的自动化设备控制系统300中的所述各模块在使用时采用与上述的图1中所述的基于六轴机械手的自动化设备控制方法一样的技术手段，并能够产生相同的技术效果，这里不再赘述。
另外，在本发明各个实施例中的各功能模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能模块的形式实现。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。
因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附关联图标记视为限制所涉及的权利要求。
需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所发明的原理和新颖特点相一致的最宽的范围。

Claims (5)

1.一种基于六轴机械手的自动化设备控制方法，其特征在于，所述方法包括：
确定六轴机械手的末端的末端坐标点，采集所述六轴机械手的当前位姿，其中，所述六轴机械手包括末端、第一连杆、第二连杆、第一连杆起点、第一连杆终点，所述当前位姿包括当前坐标点与当前角度值，其中，所述末端坐标点是指末端在下一步需要达到的坐标位置；
利用所述末端坐标点规划所述六轴机械手的目标位姿，其中，所述目标位姿包括目标坐标点与目标角度值，基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，利用所述位姿成本从所述目标位姿中选取最终位姿，其中，所述利用所述末端坐标点规划所述六轴机械手的目标位姿，包括：
获取所述六轴机械手的旋转角度范围与连杆长度；
基于所述旋转角度范围与所述连杆长度，利用下述公式构建所述末端坐标点的目标函数：
lx＝l1cosθx
ly＝l1cosθy
lz＝l1cosθz
l'x＝l2cosθ'x
l'y＝l2cosθ'y
l'z＝l2cosθ'z
l”x＝lx+l'x
l”y＝ly+l'y
l”z＝lz+l'z
l”x＝l1cosθx+l2cosθ'x
l”y＝l1cosθy+l2cosθ'y
l”z＝l1cosθz+l2cosθ'z
其中，F表示目标函数，F包括min|l”x-l”'x|、min|l”y-l”'y|及min|l”z-l”'z|，(l”'x,l”'y,l”'z)表示所述末端坐标点，(l”x,l”y,l”z)表示所述末端坐标点的待优化坐标点，l1表示所述连杆长度中第一连杆的长度，l2表示所述连杆长度中第二连杆的长度，(θx,θy,θz)表示以第一连杆起点为原点的直角坐标系中第一连杆相对于直角坐标系的偏转角度，(θ'x,θ'y,θ'z)表示以第一连杆终点为原点的直角坐标系中第二连杆相对于直角坐标系的偏转角度，lx表示以第一连杆起点为原点的直角坐标系中第一连杆在x轴上的映射长度，ly表示以第一连杆起点为原点的直角坐标系中第一连杆在y轴上的映射长度，lz表示以第一连杆起点为原点的直角坐标系中第一连杆在z轴上的映射长度，l'x表示以第一连杆终点为原点的直角坐标系中第二连杆在x轴上的映射长度，l'y表示以第一连杆终点为原点的直角坐标系中第二连杆在y轴上的映射长度，l'z表示以第一连杆终点为原点的直角坐标系中第二连杆在z轴上的映射长度，θx,θy,θz,θ'x,θ'y,θ'z属于所述旋转角度范围；
利用下述公式对所述目标函数中的角度参数进行角度更新，得到更新角度：
v'ij＝vij+c1×rand×(pbestij-xij)+c2×rand×(gbestij-xij)
x'ij＝xij+vij
其中，x'ij表示更新角度，v'ij表示对原始更新速度进行更新之后的速度，i表示θ的序号，j表示θ'的序号，vij表示θ'的原始更新速度，c1与c2表示学习因子，c1＝c2＝2，rand表示介于0～1之间的随机数，xij表示θ'的当前数值，pbestij表示θ'的邻近历史取值中F值为0的邻近历史取值，gbestij表示θ'的全部历史取值中F值为0的历史取值，θ'包括θ'x,θ'y,θ'z，θ包括θx,θy,θz；
判断所述更新角度在所述目标函数中对应的目标函数值是否为预设阈值；
当所述更新角度在所述目标函数中对应的目标函数值为所述预设阈值时，利用所述更新角度计算所述六轴机械手的目标坐标点与目标角度值；
将所述目标坐标点与所述目标角度值作为所述目标位姿；
计算所述当前位姿与所述最终位姿之间的位姿误差，构建所述六轴机械手的传递函数，利用所述位姿误差构建所述六轴机械手的PID控制器，基于所述传递函数，计算所述PID控制器的增益参数，其中，所述基于所述传递函数，计算所述PID控制器的增益参数，包括：
获取PID控制器对应的输出误差；
基于所述输出误差与所述传递函数，构建所述六轴机械手的状态方程矩阵；
构建所述PID控制器下的李雅普诺夫原模型；
对所述李雅普诺夫原模型进行模型求导，得到李雅普诺夫原模型的导数；
计算输出误差下所述PID控制器的反函数；
将所述状态方程矩阵与所述反函数代入所述李雅普诺夫原模型的导数中，得到李雅普诺夫导数模型；
判断是否可以构建所述李雅普诺夫原模型的正定矩阵；
在可以构建所述李雅普诺夫原模型的正定矩阵时，判定所述李雅普诺夫原模型大于预设阈值；
在所述李雅普诺夫原模型大于预设阈值时，构建所述李雅普诺夫导数模型的负定矩阵；
在所述PID控制器的比例增益、积分增益及微分增益符合所述负定矩阵时，确定所述PID控制器的增益参数；
基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，并采集所述六轴机械手的当前控制信号；
计算从所述当前控制信号至所述规划控制信号的信号控制风险，根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，利用所述设备控制信号对所述六轴机械手进行设备控制，得到所述六轴机械手的设备控制结果，其中，信号控制风险是指信号控制时会对六轴机械手造成的机械障碍的风险，所述计算从所述当前控制信号至所述规划控制信号的信号控制风险，包括：
查询所述当前控制信号与所述规划控制信号中每两个相邻控制信号之间的信号控制区间；
按照每个信号控制区间之间的相邻关系，随机合并所述信号控制区间，得到合并控制区间；
识别历史时段中是否存在不小于所述合并控制区间的区间；
在历史时段中存在不小于所述合并控制区间的区间时，将无风险作为所述信号控制风险；
在历史时段中不存在不小于所述合并控制区间的区间时，计算所述合并控制区间的区间长度；
识别所述历史时段中是否存在与所述区间长度一致的目标长度；
查询所述区间长度下的区间规模与所述目标长度下的目标规模；
计算所述区间规模与所述目标规模之间的规模标准差；
利用所述规模标准差构建所述区间规模的标准差阈值；
在所述区间规模大于所述标准差阈值时，将有风险作为所述信号控制风险；
在所述区间规模不大于所述标准差阈值时，将无风险作为所述信号控制风险；
所述计算所述区间规模与所述目标规模之间的规模标准差，包括：
获取所述区间规模中的区间角度差与区间坐标距离；
计算所述区间角度差与所述区间坐标距离之间的第一角度-坐标和；
获取所述目标规模中的目标角度差与目标坐标距离；
计算所述目标角度差与所述目标坐标距离之间的第二角度-坐标和；
计算所述第一角度-坐标和与所述第二角度-坐标和之间的和均值；
根据所述第一角度-坐标和、所述第二角度-坐标和及所述和均值，计算所述区间规模与所述目标规模之间的规模标准差；
所述根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，包括：
在所述信号控制风险为无风险时，保留所述无风险对应的合并控制区间，得到保留区间；
查询所述保留区间中每个相邻的保留区间之间是否区间连续；
在所述保留区间中每个相邻的保留区间之间未区间连续时，根据所述保留区间中的信号控制区间，缩短所述保留区间的区间长度，得到缩短区间，以通过所述缩短区间对齐所述保留区间中每个相邻的保留区间，得到对齐区间；
计算所述对齐区间的区间数目；
从所述区间数目中选取最小数目；
从所述对齐区间中获取所述最小数目对应的目标对齐区间；
将所述目标对齐区间的边界上的规划控制信号作为所述设备控制信号。
2.根据权利要求1所述的方法，其特征在于，所述基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，包括：
获取所述目标位姿中的目标坐标点与目标角度值；
利用下述公式计算所述当前坐标点至所述目标坐标点的坐标成本：
U＝u1+u2
其中，U表示坐标成本，u1表示当前坐标点中第一连杆终点的坐标与目标坐标点中第一连杆终点的坐标之间的差值的绝对值，u2表示当前坐标点中末端的坐标与末端坐标点之间的差值的绝对值；
利用下述公式计算所述当前角度值至所述目标坐标点的角度成本：
V＝v1+v2
其中，V表示角度成本，v1表示当前角度值中第一连杆终点的角度与目标角度值中第一连杆终点的角度之间的差值的绝对值，v2表示当前角度值中末端的角度与末端角度值之间的差值的绝对值；
将所述坐标成本与所述角度成本之和作为所述当前位姿至所述目标位姿的位姿成本。
3.根据权利要求1所述的方法，其特征在于，所述构建所述六轴机械手的传递函数，包括：
利用下述公式构建所述六轴机械手的传递函数：
p(s)*(s2+αs+β)＝γ*q(s)
s2p(s)+αsp(s)+βp(s)＝γq(s)
其中，表示所述六轴机械手的传递函数，t表示时域中的时间自变量，p(t)表示在PID控制器的控制下六轴机械手被调整之后的位姿，q(t)表示PID控制器向六轴机械手输入的位姿控制信号，α、β、γ表示常数参数，s表示对p(t)进行拉普拉斯变换时将t进行变换而得到的自变量，p(s)表示p(t)的拉普拉斯变换形式，q(s)表示q(t)的拉普拉斯变换形式。
4.根据权利要求1所述的方法，其特征在于，所述基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，包括：
利用所述增益参数调整所述PID控制器，得到调整控制器；
将所述位姿误差代入所述调整控制器中；
在所述调整控制器中输出所述位姿误差对应的规划控制信号。
5.一种基于六轴机械手的自动化设备控制系统，其特征在于，所述系统包括：
位姿采集模块，用于确定六轴机械手的末端的末端坐标点，采集所述六轴机械手的当前位姿，其中，所述六轴机械手包括末端、第一连杆、第二连杆、第一连杆起点、第一连杆终点，所述当前位姿包括当前坐标点与当前角度值，其中，所述末端坐标点是指末端在下一步需要达到的坐标位置；
位姿选取模块，用于利用所述末端坐标点规划所述六轴机械手的目标位姿，其中，所述目标位姿包括目标坐标点与目标角度值，基于所述当前坐标点与所述当前角度值，计算所述当前位姿至所述目标位姿的位姿成本，利用所述位姿成本从所述目标位姿中选取最终位姿，其中，所述利用所述末端坐标点规划所述六轴机械手的目标位姿，包括：
获取所述六轴机械手的旋转角度范围与连杆长度；
基于所述旋转角度范围与所述连杆长度，利用下述公式构建所述末端坐标点的目标函数：
lx＝l1cosθx
ly＝l1cosθy
lz＝l1cosθz
l'x＝l2cosθ'x
l'y＝l2cosθ'y
l'z＝l2cosθ'z
l”x＝lx+l'x
l”y＝ly+l'y
l”z＝lz+l'z
l”x＝l1cosθx+l2cosθ'x
l”y＝l1cosθy+l2cosθ'yl”z＝l1cosθz+l2cosθ'z
其中，F表示目标函数，F包括min|l”x-l”'x|、min|l”y-l”'y|及min|l”z-l”'z|，(l”'x,l”'y,l”'z)表示所述末端坐标点，(l”x,l”y,l”z)表示所述末端坐标点的待优化坐标点，l1表示所述连杆长度中第一连杆的长度，l2表示所述连杆长度中第二连杆的长度，(θx,θy,θz)表示以第一连杆起点为原点的直角坐标系中第一连杆相对于直角坐标系的偏转角度，(θ'x,θ'y,θ'z)表示以第一连杆终点为原点的直角坐标系中第二连杆相对于直角坐标系的偏转角度，lx表示以第一连杆起点为原点的直角坐标系中第一连杆在x轴上的映射长度，ly表示以第一连杆起点为原点的直角坐标系中第一连杆在y轴上的映射长度，lz表示以第一连杆起点为原点的直角坐标系中第一连杆在z轴上的映射长度，l'x表示以第一连杆终点为原点的直角坐标系中第二连杆在x轴上的映射长度，l'y表示以第一连杆终点为原点的直角坐标系中第二连杆在y轴上的映射长度，l'z表示以第一连杆终点为原点的直角坐标系中第二连杆在z轴上的映射长度，θx,θy,θz,θ'x,θ'y,θ'z属于所述旋转角度范围；
利用下述公式对所述目标函数中的角度参数进行角度更新，得到更新角度：
v'ij＝vij+c1×rand×(pbestij-xij)+c2×rand×(gbestij-xij)
x'ij＝xij+vij
其中，x'ij表示更新角度，v'ij表示对原始更新速度进行更新之后的速度，i表示θ的序号，j表示θ'的序号，vij表示θ'的原始更新速度，c1与c2表示学习因子，c1＝c2＝2，rand表示介于0～1之间的随机数，xij表示θ'的当前数值，pbestij表示θ'的邻近历史取值中F值为0的邻近历史取值，gbestij表示θ'的全部历史取值中F值为0的历史取值，θ'包括θ'x,θ'y,θ'z，θ包括θx,θy,θz；
判断所述更新角度在所述目标函数中对应的目标函数值是否为预设阈值；
当所述更新角度在所述目标函数中对应的目标函数值为所述预设阈值时，利用所述更新角度计算所述六轴机械手的目标坐标点与目标角度值；
将所述目标坐标点与所述目标角度值作为所述目标位姿；
参数计算模块，用于计算所述当前位姿与所述最终位姿之间的位姿误差，构建所述六轴机械手的传递函数，利用所述位姿误差构建所述六轴机械手的PID控制器，基于所述传递函数，计算所述PID控制器的增益参数，其中，所述基于所述传递函数，计算所述PID控制器的增益参数，包括：
获取PID控制器对应的输出误差；
基于所述输出误差与所述传递函数，构建所述六轴机械手的状态方程矩阵；
构建所述PID控制器下的李雅普诺夫原模型；
对所述李雅普诺夫原模型进行模型求导，得到李雅普诺夫原模型的导数；
计算输出误差下所述PID控制器的反函数；
将所述状态方程矩阵与所述反函数代入所述李雅普诺夫原模型的导数中，得到李雅普诺夫导数模型；
判断是否可以构建所述李雅普诺夫原模型的正定矩阵；
在可以构建所述李雅普诺夫原模型的正定矩阵时，判定所述李雅普诺夫原模型大于预设阈值；
在所述李雅普诺夫原模型大于预设阈值时，构建所述李雅普诺夫导数模型的负定矩阵；
在所述PID控制器的比例增益、积分增益及微分增益符合所述负定矩阵时，确定所述PID控制器的增益参数；
信号采集模块，用于基于所述增益参数与所述位姿误差，利用所述PID控制器输出所述六轴机械手的规划控制信号，并采集所述六轴机械手的当前控制信号；
设备控制模块，用于计算从所述当前控制信号至所述规划控制信号的信号控制风险，根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，利用所述设备控制信号对所述六轴机械手进行设备控制，得到所述六轴机械手的设备控制结果，其中，信号控制风险是指信号控制时会对六轴机械手造成的机械障碍的风险，所述计算从所述当前控制信号至所述规划控制信号的信号控制风险，包括：
查询所述当前控制信号与所述规划控制信号中每两个相邻控制信号之间的信号控制区间；
按照每个信号控制区间之间的相邻关系，随机合并所述信号控制区间，得到合并控制区间；
识别历史时段中是否存在不小于所述合并控制区间的区间；
在历史时段中存在不小于所述合并控制区间的区间时，将无风险作为所述信号控制风险；
在历史时段中不存在不小于所述合并控制区间的区间时，计算所述合并控制区间的区间长度；
识别所述历史时段中是否存在与所述区间长度一致的目标长度；
查询所述区间长度下的区间规模与所述目标长度下的目标规模；
计算所述区间规模与所述目标规模之间的规模标准差；
利用所述规模标准差构建所述区间规模的标准差阈值；
在所述区间规模大于所述标准差阈值时，将有风险作为所述信号控制风险；
在所述区间规模不大于所述标准差阈值时，将无风险作为所述信号控制风险；
所述计算所述区间规模与所述目标规模之间的规模标准差，包括：
获取所述区间规模中的区间角度差与区间坐标距离；
计算所述区间角度差与所述区间坐标距离之间的第一角度-坐标和；
获取所述目标规模中的目标角度差与目标坐标距离；
计算所述目标角度差与所述目标坐标距离之间的第二角度-坐标和；
计算所述第一角度-坐标和与所述第二角度-坐标和之间的和均值；
根据所述第一角度-坐标和、所述第二角度-坐标和及所述和均值，计算所述区间规模与所述目标规模之间的规模标准差；
所述根据所述信号控制风险，从所述规划控制信号中筛选设备控制信号，包括：
在所述信号控制风险为无风险时，保留所述无风险对应的合并控制区间，得到保留区间；
查询所述保留区间中每个相邻的保留区间之间是否区间连续；
在所述保留区间中每个相邻的保留区间之间未区间连续时，根据所述保留区间中的信号控制区间，缩短所述保留区间的区间长度，得到缩短区间，以通过所述缩短区间对齐所述保留区间中每个相邻的保留区间，得到对齐区间；
计算所述对齐区间的区间数目；
从所述区间数目中选取最小数目；
从所述对齐区间中获取所述最小数目对应的目标对齐区间；
将所述目标对齐区间的边界上的规划控制信号作为所述设备控制信号。
CN202411134713.7A 2024-08-19 2024-08-19 基于六轴机械手的自动化设备控制方法及系统 Active CN118789552B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202411134713.7A CN118789552B (zh) 2024-08-19 2024-08-19 基于六轴机械手的自动化设备控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202411134713.7A CN118789552B (zh) 2024-08-19 2024-08-19 基于六轴机械手的自动化设备控制方法及系统

Publications (2)

Publication Number Publication Date
CN118789552A CN118789552A (zh) 2024-10-18
CN118789552B true CN118789552B (zh) 2025-05-30

Family

ID=93025373

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202411134713.7A Active CN118789552B (zh) 2024-08-19 2024-08-19 基于六轴机械手的自动化设备控制方法及系统

Country Status (1)

Country Link
CN (1) CN118789552B (zh)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105159096A (zh) * 2015-10-10 2015-12-16 北京邮电大学 一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法
CN105301966A (zh) * 2015-11-27 2016-02-03 上海无线电设备研究所 基于输入受限式自激发驱动的多机器人协同控制方法

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001222309A (ja) * 2000-02-10 2001-08-17 Yaskawa Electric Corp ロボット制御装置
FR3110257A1 (fr) * 2020-05-12 2021-11-19 Institut National De Recherche En Informatique Et En Automatique (Inria) Utilisation de l’homogénéité généralisée pour améliorer une commande PID
CN113119116B (zh) * 2021-03-22 2023-01-31 深圳市优必选科技股份有限公司 一种机械臂运动规划方法、装置、可读存储介质及机械臂
JP7613738B2 (ja) * 2021-06-03 2025-01-15 国立研究開発法人産業技術総合研究所 制御方法および制御装置
CN114161399B (zh) * 2021-12-20 2023-08-18 大连理工大学 一种六自由度机械臂笛卡尔空间运动目标实时跟踪控制方法
CN114355929B (zh) * 2021-12-30 2024-04-09 华中科技大学 轨迹柔顺切换的移动机器人自适应避障控制方法
US12393166B2 (en) * 2022-06-30 2025-08-19 Advanced Energy Industries, Inc. Adaptive PID controller
CN115236973B (zh) * 2022-07-27 2025-04-15 浙江工业大学 基于pso李雅普诺夫函数的agv轨迹跟踪控制方法
CN116079717A (zh) * 2022-12-15 2023-05-09 武汉擎朗智能科技有限公司 机器人控制方法、机器人及计算机可读存储介质
EP4659093A2 (en) * 2023-02-03 2025-12-10 Lucomm Technologies, Inc. Robotic control system

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105159096A (zh) * 2015-10-10 2015-12-16 北京邮电大学 一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法
CN105301966A (zh) * 2015-11-27 2016-02-03 上海无线电设备研究所 基于输入受限式自激发驱动的多机器人协同控制方法

Also Published As

Publication number Publication date
CN118789552A (zh) 2024-10-18

Similar Documents

Publication Publication Date Title
CN110275436A (zh) 一种多单臂机械手的rbf神经网络自适应控制方法
CN107984472B (zh) 一种用于冗余度机械臂运动规划的变参神经求解器设计方法
CN109676610B (zh) 一种断路器装配机器人及其实现工作轨迹优化的方法
CN114523477B (zh) 关节位姿的校准方法、系统及存储介质
CN111722628B (zh) 一种多智能体轨迹更新迭代学习的点到点跟踪控制方法
CN109144047B (zh) 一种基于多智能体系统的有限时间编队控制方法
CN113664835B (zh) 机器人自动手眼标定方法与系统
CN113814983B (zh) 一种多单臂机械手系统控制方法及系统
CN113610968B (zh) 目标检测模型的更新方法及装置
CN112428273A (zh) 一种考虑机械臂物理约束和模型未知的控制方法及系统
CN113910218A (zh) 基于运动学与深度神经网络融合的机器人标定方法和装置
CN118809604B (zh) 一种基于视觉伺服的机器人协同控制方法及相关装置
CN113070877A (zh) 一种面向七轴机械臂视觉示教的可变姿态映射方法
CN117067209A (zh) 一种协作机器人多轴孔装配方法、系统、电子设备及介质
CN116079723A (zh) 基于视觉的机器人抓取和装配技能深度强化学习方法
CN112070835A (zh) 机械臂位姿预测方法、装置、存储介质及电子设备
CN118789552B (zh) 基于六轴机械手的自动化设备控制方法及系统
CN120791792B (zh) 多轴机械臂运动轨迹智能规划方法
CN117452975A (zh) 一种四旋翼无人机集群的保性能协同编队控制设计方法
CN111435244A (zh) 一种回环闭合方法、装置及机器人
Luo et al. End‐Effector Pose Estimation in Complex Environments Using Complementary Enhancement and Adaptive Fusion of Multisensor
CN117392241B (zh) 自动驾驶中的传感器标定方法、装置及电子设备
CN113910221B (zh) 一种机械臂自主运动规划方法、装置、设备及存储介质
CN118710730A (zh) 关节置换手术机器人手到眼型工具坐标系标定方法及装置
Jin et al. Exploration of Key Technologies of Intelligent Inspection Robots in the Application of Automatic Energy Meter Verification Line

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