CN116922399B - 机械臂重力补偿方法、装置、电子设备及存储介质 - Google Patents
机械臂重力补偿方法、装置、电子设备及存储介质 Download PDFInfo
- Publication number
- CN116922399B CN116922399B CN202311189595.5A CN202311189595A CN116922399B CN 116922399 B CN116922399 B CN 116922399B CN 202311189595 A CN202311189595 A CN 202311189595A CN 116922399 B CN116922399 B CN 116922399B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- gravity compensation
- gravity
- data
- joint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000005484 gravity Effects 0.000 title claims abstract description 302
- 238000000034 method Methods 0.000 title claims abstract description 77
- 239000011159 matrix material Substances 0.000 claims abstract description 53
- 230000009466 transformation Effects 0.000 claims abstract description 45
- 238000001914 filtration Methods 0.000 claims abstract description 22
- 238000004364 calculation method Methods 0.000 claims description 17
- 238000004590 computer program Methods 0.000 claims description 10
- 238000012937 correction Methods 0.000 claims description 7
- 238000010276 construction Methods 0.000 claims description 2
- 238000004891 communication Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 230000009471 action Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012821 model calculation Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
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/1628—Programme controls characterised by the control loop
- B25J9/1638—Programme controls characterised by the control loop compensation for arm bending/inertia, pay load weight/inertia
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
Abstract
本申请属于机械臂重力补偿的技术领域，公开了一种机械臂重力补偿方法、装置、电子设备及存储介质，该方法包括：获取机械臂的结构数据，根据结构数据，构建机械臂的D‑H坐标系，基于D‑H坐标系，确定机械臂中相邻两个关节间的齐次变换矩阵，根据齐次变换矩阵，结合各连杆的质量和质心位置，计算得到机械臂当前位姿所作用在每个关节上的重力补偿值，运用卡尔曼滤波方法，结合实时获取的机械臂在零速度模式下的重力补偿电流数据，对重力补偿值进行修正，得到机械臂的最优重力补偿数据，通过卡尔曼滤波方法，结合机械臂在零速度模式下的重力补偿电流数据，对机械臂进行重力补偿，提高了机械臂的重力补偿效率。
Description
技术领域
本申请涉及机械臂重力补偿的技术领域，具体而言，涉及一种机械臂重力补偿方法、装置、电子设备及存储介质。
背景技术
在六轴机械臂的运行与控制中，重力补偿是一项重要的技术，通过在机械臂操作过程中对各关节和连杆的重力进行补偿，以减轻对机械臂电机和结构的负载，可以有效提高其精确性和可靠性，使机械臂更加高效和可靠地完成各种任务。
现有的重力补偿技术主要包括动力学模型计算方法。在动力学模型计算中，通过计算机器人的连杆质量、关节角度，来计算获取整体机械臂对每个关节的重力力矩，从而计算出相应的补偿力矩，以抵消重力的影响。
在动力学模型计算的方法中，由于机械臂末端的负载质量无法测量，会导致计算结果与实际重力存在偏差，当机械臂搭载较重载荷时，会导致偏差较大，从而使控制不稳定甚至失稳。
因此，为了解决现有的机械臂重力补偿方法在机械臂负载的情况下会导致重力补偿出现偏差的技术问题，亟需一种机械臂重力补偿方法、装置、电子设备及存储介质。
发明内容
本申请的目的在于提供一种机械臂重力补偿方法、装置、电子设备及存储介质，通过卡尔曼滤波方法，结合机械臂在零速度模式下的重力补偿电流数据，对机械臂进行重力补偿，解决现有的机械臂重力补偿方法在机械臂负载的情况下会导致重力补偿出现偏差的问题，可以在负载或环境发生变化时对机械臂的重力补偿值进行实时调整，保证机械臂的控制稳定与运行平稳，使机械臂可以在高精度和高速度的工况下良好运行，提高了机械臂的重力补偿效率。
第一方面，本申请提供了一种机械臂重力补偿方法，用于对机械臂进行重力补偿，包括步骤：
获取机械臂的结构数据；
根据所述结构数据，构建所述机械臂的D-H坐标系；
基于所述D-H坐标系，确定所述机械臂中相邻两个关节间的齐次变换矩阵；
根据所述齐次变换矩阵，结合各连杆的质量和质心位置，计算得到所述机械臂当前位姿所作用在每个关节上的重力补偿值；
运用卡尔曼滤波方法，结合实时获取的所述机械臂在零速度模式下的重力补偿电流数据，对所述重力补偿值进行修正，得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
本申请提供的机械臂重力补偿方法可以实现对机械臂进行重力补偿，通过卡尔曼滤波方法，结合机械臂在零速度模式下的重力补偿电流数据，对机械臂进行重力补偿，解决现有的机械臂重力补偿方法在机械臂负载的情况下会导致重力补偿出现偏差的问题，可以在负载或环境发生变化时对机械臂的重力补偿值进行实时调整，保证机械臂的控制稳定与运行平稳，使机械臂可以在高精度和高速度的工况下良好运行，提高了机械臂的重力补偿效率。
可选地，基于所述D-H坐标系，确定所述机械臂中相邻两个关节间的齐次变换矩阵，包括：
定义所述机械臂在所述D-H坐标系下的关节参数和连杆参数；
基于所述关节参数和所述连杆参数，确定所述机械臂中相邻两个关节间的齐次变换矩阵。
可选地，根据所述齐次变换矩阵，结合各连杆的质量和质心位置，计算得到所述机械臂当前位姿所作用在每个关节上的重力补偿值，包括：
根据所述齐次变换矩阵、所述各连杆的质量和质心位置，计算所述机械臂中各连杆作用于各关节的重力力矩；
基于所述齐次变换矩阵和所述重力力矩，计算得到所述机械臂当前位姿所作用在每个关节上的重力补偿值。
本申请提供的机械臂重力补偿方法可以实现对机械臂进行重力补偿，通过齐次变换矩阵、各连杆的质量和质心位置，能够确定机械臂当前位姿所作用在每个关节上的重力补偿值，得到理论上的机械臂的重力补偿数据，有利于提高机械臂的重力补偿效率。
可选地，运用卡尔曼滤波方法，结合实时获取的所述机械臂在零速度模式下的重力补偿电流数据，对所述重力补偿值进行修正，得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据，包括：
采用先标定后插值的方法，获取所述机械臂在零速度模式下的重力补偿电流数据；
根据预设的随机噪声向量，运用线性变换方式，将所述重力补偿电流数据转换为重力补偿向量；
基于所述重力补偿向量，结合所述重力补偿值的卡尔曼增益数据，计算得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
本申请提供的机械臂重力补偿方法可以实现对机械臂进行重力补偿，通过卡尔曼滤波方法，结合机械臂在零速度模式下的重力补偿电流数据，对机械臂的理论重力补偿值进行修正，得到机械臂的最优重力补偿数据，提高了机械臂的重力补偿效率。
可选地，采用先标定后插值的方法，获取所述机械臂在零速度模式下的重力补偿电流数据，包括：
对所述机械臂中受重力影响的关节进行位置标定，得到所述机械臂的标定位置；
获取零速度模式下所述机械臂中标定位置的标定电流；
通过插值计算方法，根据所述标定电流，计算得到所述机械臂在零速度模式下的重力补偿电流数据。
可选地，基于所述重力补偿向量，结合所述重力补偿值的卡尔曼增益数据，计算得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据，包括：
以预设的重力误差阈值为依据，计算所述重力补偿值的卡尔曼增益数据；
基于所述卡尔曼增益数据，更新所述重力误差阈值，得到最优重力补偿方差；
根据所述最优重力补偿方差、所述重力补偿向量和所述卡尔曼增益数据，计算得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
可选地，基于所述卡尔曼增益数据，更新所述重力误差阈值，得到最优重力补偿方差之后，还包括：
将所述最优重力补偿方差设置为所述重力误差阈值。
第二方面，本申请提供了一种机械臂重力补偿装置，用于对机械臂进行重力补偿，包括：
获取模块，用于获取机械臂的结构数据；
构建模块，用于根据所述结构数据，构建所述机械臂的D-H坐标系；
矩阵模块，用于基于所述D-H坐标系，确定所述机械臂中相邻两个关节间的齐次变换矩阵；
计算模块，用于根据所述齐次变换矩阵，结合各连杆的质量和质心位置，计算得到所述机械臂当前位姿所作用在每个关节上的重力补偿值；
修正模块，用于运用卡尔曼滤波方法，结合实时获取的所述机械臂在零速度模式下的重力补偿电流数据，对所述重力补偿值进行修正，得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
该机械臂重力补偿装置，通过卡尔曼滤波方法，结合机械臂在零速度模式下的重力补偿电流数据，对机械臂进行重力补偿，解决现有的机械臂重力补偿方法在机械臂负载的情况下会导致重力补偿出现偏差的问题，可以在负载或环境发生变化时对机械臂的重力补偿值进行实时调整，保证机械臂的控制稳定与运行平稳，使机械臂可以在高精度和高速度的工况下良好运行，提高了机械臂的重力补偿效率。
第三方面，本申请提供了一种电子设备，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如前文所述机械臂重力补偿方法中的步骤。
第四方面，本申请提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述机械臂重力补偿方法中的步骤。
有益效果：
本申请提供的机械臂重力补偿方法、装置、电子设备及存储介质，通过卡尔曼滤波方法，结合机械臂在零速度模式下的重力补偿电流数据，对机械臂进行重力补偿，解决现有的机械臂重力补偿方法在机械臂负载的情况下会导致重力补偿出现偏差的问题，可以在负载或环境发生变化时对机械臂的重力补偿值进行实时调整，保证机械臂的控制稳定与运行平稳，使机械臂可以在高精度和高速度的工况下良好运行，提高了机械臂的重力补偿效率。
附图说明
图1为本申请实施例提供的机械臂重力补偿方法的流程图。
图2为本申请实施例提供的机械臂重力补偿装置的结构示意图。
图3为本申请实施例提供的电子设备的结构示意图。
图4为六轴机械臂的D-H坐标系的示意图。
标号说明：1、获取模块；2、构建模块；3、矩阵模块；4、计算模块；5、修正模块；301、处理器；302、存储器；303、通信总线。
具体实施方式
下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
请参照图1，图1是本申请一些实施例中的一种机械臂重力补偿方法，用于对机械臂进行重力补偿，包括：
步骤S101，获取机械臂的结构数据；
步骤S102，根据结构数据，构建机械臂的D-H坐标系；
步骤S103，基于D-H坐标系，确定机械臂中相邻两个关节间的齐次变换矩阵；
步骤S104，根据齐次变换矩阵，结合各连杆的质量和质心位置，计算得到机械臂当前位姿所作用在每个关节上的重力补偿值；
步骤S105，运用卡尔曼滤波方法，结合实时获取的机械臂在零速度模式下的重力补偿电流数据，对重力补偿值进行修正，得到机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
该机械臂重力补偿方法，通过卡尔曼滤波方法，结合机械臂在零速度模式下的重力补偿电流数据，对机械臂进行重力补偿，解决现有的机械臂重力补偿方法在机械臂负载的情况下会导致重力补偿出现偏差的问题，可以在负载或环境发生变化时对机械臂的重力补偿值进行实时调整，保证机械臂的控制稳定与运行平稳，使机械臂可以在高精度和高速度的工况下良好运行，提高了机械臂的重力补偿效率。
具体地，在步骤S101中，获取机械臂的结构数据，结构数据包括机械臂的关节数量、关节类型、臂长等数据。
具体地，在步骤S102中，根据结构数据，构建机械臂的D-H坐标系。对机械臂整体建立D-H坐标系，能够方便后续的计算。D-H坐标系的优点在于它可以简化机器人的运动学分析，使得机器人的运动学问题可以转化为简单的矩阵运算。同时，D-H 坐标系也可以用于描述机器人的姿态，即机器人的位置和朝向。在机器人的运动学分析中，D-H坐标系可以用于描述机器人的关节角度和末端执行器的位置和姿态。通过对D-H坐标系的定义，可以得到机器人的正运动学方程和逆运动学方程，从而实现机器人的运动控制。
例如，如图4所示，图4为六轴机械臂的D-H坐标系的示意图，以图4所示位姿的六轴机械臂为例，对六轴机械臂建立D-H坐标系，其中，设定关节i（机械臂的第i个关节，根据机械臂的关节数（轴数）确定i的大小，如机械臂为六轴机械臂，则确定i为1，2，3，4，5，6）的转动轴线方向为，关节/>的/>轴为本关节/>轴与前一个关节/>轴的叉乘，即，并使/>和/>相交于关节原点/>，建立空间直角坐标系/>，最终使六个关节建立六个直角坐标系/>。
具体地，在步骤S103中，基于D-H坐标系，确定机械臂中相邻两个关节间的齐次变换矩阵，包括：
定义机械臂在D-H坐标系下的关节参数和连杆参数；
基于关节参数和连杆参数，确定机械臂中相邻两个关节间的齐次变换矩阵。
在步骤S103中，机械臂在D-H坐标系下的关节参数包括关节角和关节偏置距离，机械臂在D-H坐标系下的连杆参数包括连杆长度和连杆扭转角，定义机械臂在D-H坐标系下的各关节及连杆的参数，具体如下所示：
1）定义关节角为，关节角表示沿/>轴，从/>轴到/>轴的夹角（i为第i个关节）。
2）定义关节偏置距离为，关节偏置距离表示沿/>轴，从/>轴到/>轴的距离。
3）定义连杆长度为，连杆长度表示沿/>轴，从/>轴到/>轴的距离。
4）定义连杆扭转角为，连杆扭转角表示沿/>轴，从/>轴到/>轴的夹角。
基于关节参数和连杆参数，确定机械臂中相邻两个关节间的齐次变换矩阵。
例如，以图4所示位姿的六轴机械臂为例，计算六轴机械臂中相邻两个关节间的齐次变换矩阵，齐次变换矩阵的计算方式具体为：
；
其中，为机械臂中相邻两个关节间的齐次变换矩阵，即第i个关节和第i+1个关节的齐次变换矩阵，i为第i个关节，/>为第i个关节的关节角，/>为第i个关节的关节偏置距离，/>为第i个关节的连杆长度，/>为第i个关节的连杆扭转角。
具体地，在步骤S104中，根据齐次变换矩阵，结合各连杆的质量和质心位置，计算得到机械臂当前位姿所作用在每个关节上的重力补偿值，包括：
根据齐次变换矩阵、各连杆的质量和质心位置，计算机械臂中各连杆作用于各关节的重力力矩；
基于齐次变换矩阵和重力力矩，计算得到机械臂当前位姿所作用在每个关节上的重力补偿值。
在步骤S104中，根据厂家给出或自行测量的各连杆质量和质心位置，确定机械臂各关节在D-H坐标系的质心位置和质量大小，根据齐次变换矩阵计算机械臂中各连杆作用于各关节的重力力矩。
例如，以图4所示位姿的六轴机械臂为例，根据厂家给出或自行测量的各连杆质量和质心位置，确定机械臂各关节在D-H坐标系的质量和质心位置/>，计算六轴机械臂中各连杆作用于各关节的重力力矩/>，其中，M为重力力矩，i为六轴机械臂的第i个关节，j为六轴机械臂的第j个连杆，/>为关节i（第i个关节）的质量大小（关节i的质量相当于对应连杆的质量），/>为关节i在D-H坐标系的质心位置（关节i的质心位置相当于对应连杆的质心位置），重力力矩的计算过程具体如下所示：
1）计算六轴机械臂中连杆j作用于关节1的重力力矩：
；
其中，为连杆j（第j个连杆）作用于关节1的重力力矩，/>为关节i的质量大小，g为重力加速度，/>为关节i在D-H坐标系的质心位置，其中，，/>为关节i质心位置的横坐标，/>为关节i质心位置的纵坐标，/>为关节i质心位置的竖坐标，/>中的1为质心位置从坐标系拓展到矩阵后用于方便计算而设置的数值，该数值没有实际意义。
2）计算六轴机械臂中连杆j作用于关节2的重力力矩：
；
3）各连杆作用于关节3的重力力矩/>为：
；
4）各连杆作用于关节4的重力力矩/>为：
；
5）各连杆作用于关节5的重力力矩/>为：
；
6）连杆6作用于关节6的重力力矩为：
。
在步骤S104中，由齐次变换矩阵和机械臂中各连杆作用于各关节的重力力矩，可计算得到机械臂当前位姿所作用在每个关节上的重力力矩，即得到机械臂当前位姿所作用在每个关节上的重力补偿值。
例如，以图4所示位姿的六轴机械臂为例，机械臂当前位姿所作用在每个关节上的重力力矩具体为：
；
其中，为机械臂当前位姿所作用在第i个关节上的重力力矩（重力补偿值）。
将机械臂当前位姿所作用在每个关节上的重力补偿值设置为重力补偿数据中重力力矩状态的初始值，即得到/>，其中，/>表示第n时刻预测的机械臂当前位姿在第n时刻的重力补偿值，/>表示第n时刻预测的机械臂当前位姿在初始时刻的重力补偿值，T为转置符号，表示转置矩阵。
具体地，在步骤S105中，运用卡尔曼滤波方法，结合实时获取的机械臂在零速度模式下的重力补偿电流数据，对重力补偿值进行修正，得到机械臂当前位姿所作用在每个关节上的最优重力补偿数据，包括：
采用先标定后插值的方法，获取机械臂在零速度模式下的重力补偿电流数据；
根据预设的随机噪声向量，运用线性变换方式，将重力补偿电流数据转换为重力补偿向量；
基于重力补偿向量，结合重力补偿值的卡尔曼增益数据，得到机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
具体地，在步骤S105中，采用先标定后插值的方法，获取机械臂在零速度模式下的重力补偿电流数据，包括：
对机械臂中受重力影响的关节进行位置标定，得到机械臂的标定位置；
获取零速度模式下机械臂中标定位置的标定电流；
通过插值计算方法，根据标定电流，计算得到机械臂在零速度模式下的重力补偿电流数据。
在步骤S105中，由于仅通过机械臂的动力学模型（重力力矩）对重力补偿值进行计算，会导致重力补偿值出现部分偏差，且不能随着负载和环境的改变而进行修正，因此，需要对重力补偿值进行修正和调整，通过机械臂的伺服驱动器中的零速度模式，即可将机械臂稳定在当前的位姿，且每个关节电机的力矩均用于将机械臂保持稳定，即每个关节电机的输出力矩等于机械臂作用的重力力矩，记录当前每个关节电机的电流。
由于电流采样需要机械臂处于零速度模式的静止状态，在机械臂的实际运行中，不可能在每一时刻都对机械臂电机进行零速度模式下的电流辨识，因此，本方法采用先标定后插值的方法，先对机械臂中受重力影响的关节进行位置标定（位置标定的数量可根据实际情况进行设置），得到机械臂的标定位置，获取零速度模式下机械臂中标定位置的标定电流，再通过插值计算方法，根据标定电流，计算得到机械臂在零速度模式下的重力补偿电流数据，通过该方法，可获取到机械臂在运行的每个时刻中每个关节电机的零速度电流测量值（重力补偿电流数据）。
例如，以图4所示位姿的六轴机械臂为例，根据该六轴机械臂的构型和安装方式，从重力力矩来看，关节1、关节4、关节6在运行中不受重力影响，因此不参与标定与插值（若更换构型或更换安装方式则需要重新对机械臂进行受力分析，如当关节1的固定平面为垂直安装时，不受重力影响的关节为关节4和关节6）。从关节2、关节3、关节5中各设置其最小与最大工作角度，共同组成工作空间，表示为,/>,/>，在各关节的工作角度内，平均选取5个点，即/>，其中，/>为设置在关节2的标定位置，b为设置在关节3的标定位置，c为设置在关节5的标定位置，对于三个关节各5个点的组合，可组合成机械臂的标定位置P，标定位置P可表示为，其中，正整数l取值范围为[1,5]，正整数m取值范围为[1,5],正整数k取值范围为[1,5]，标定位置共有125个。
在125个标定位置中，以零速度模式对各轴电机电流进行采集辨识，得到重力补偿电流数据，其中，I2为关节2标定位置的电流，I3为关节3标定位置的电流，I5为关节5标定位置的电流（由于图4所示位姿的六轴机械臂中关节1、关节4、关节6在运行中不受重力影响，因此关节1、关节4、关节6不参与标定与插值，所以关节1、关节4、关节6的电流数值为零），标定完成后，若要计算某位置的重力补偿电流值数据，就要进行插值计算。首先选取各标定点/>的相邻区间/>,/>,，使得/>,/>,/>，随后对其进行三维线性插值，结果为：
；
其中，Z为电流测量值，即重力补偿电流数据。
修正模块5在执行时，设定随机噪声向量v，以线性变换的方式，将重力补偿电流数据转换为重力补偿向量，重力补偿向量的转换方程为：
；
其中，Z为重力补偿电流数据，H为观测矩阵，为重力补偿向量，v为预设的随机噪声向量。随机噪声向量可根据机械臂的驱动器内部电流采样电路的误差值进行设置。观测矩阵可根据实际需要进行设置。
具体地，修正模块5在基于重力补偿向量，结合重力补偿值的卡尔曼增益数据，得到机械臂当前位姿所作用在每个关节上的最优重力补偿数据的时候，执行：
以预设的重力误差阈值为依据，计算重力补偿值的卡尔曼增益数据
基于卡尔曼增益数据，更新重力误差阈值，得到最优重力补偿方差；
根据最优重力补偿方差、重力补偿向量和卡尔曼增益数据，计算得到机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
修正模块5在执行时，在机械臂的伺服驱动器进行零速度模式的电流辨识的同时，可计算重力补偿值所需的卡尔曼增益数据，卡尔曼增益数据的计算方式具体为：
；
其中，n为第n个时刻，一般指当前时刻。Kn为当前时刻的卡尔曼增益数据（第n个时刻的卡尔曼增益数据），为观测矩阵H的转置矩阵。/>为上一时刻的重力补偿方差，初始时刻的重力补偿方差即为预设的重力误差阈值，根据厂家提供的机械臂数据中的重力误差或自行测量得到的重力误差计算得到重力补偿方差的初始值（初始时刻的重力补偿方差），即初始值/>，其中，e为重力误差，eT为重力误差的转置矩阵，E表示期望值计算。R为噪声矩阵，/>，vT为随机噪声向量的转置矩阵。
然后便可更新重力力矩状态和重力补偿方差，重力补偿方差的更新过程具体为：
；
重力力矩状态的更新过程具体为：
；
其中，为当前时刻的重力力矩状态（机械臂当前位姿在当前时刻所作用在每个关节上的重力力矩），/>为上一个时刻的重力力矩状态，/>为当前时刻的重力补偿方差， L为i阶单位矩阵，表示i*i的单位矩阵，L根据机械臂的关节数量设定。
重力力矩状态的更新过程表示用上一时刻的重力力矩状态计算当前时刻的重力力矩状态，用上上一时刻的重力力矩状态计算上一时刻的重力力矩状态，一直追溯到第一个时刻（机械臂当前位姿在第一个时刻所作用在每个关节上的重力力矩），即追溯到重力力矩状态的初始值，同理，重力补偿方差也一直追溯到初始值。
由此，更新后的重力力矩状态和重力补偿方差即为最优重力力矩状态和最优重力补偿方差，确定最优重力力矩状态为机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
可将计算得到的最优重力补偿数据作为机械臂驱动器中电流环的前馈输入，加入到机械臂电机的控制程式中，增加机械臂电机控制的稳定性，以及机械臂运行的精确性、稳定性。
具体地，修正模块5在基于卡尔曼增益数据，更新重力误差阈值，得到最优重力补偿方差之后，执行：
将最优重力补偿方差设置为重力误差阈值。
将最优重力补偿方差设置为重力误差阈值，即在后续时刻机械臂的重力补偿计算时，将最优重力补偿方差作为重力误差阈值进行使用，最优重力补偿方差可用于后续时刻的卡尔曼滤波计算，也可以用于计算机械臂在下一个位姿所作用在每个关节上的最优重力补偿数据，随着卡尔曼滤波次数（卡尔曼增益数据的计算次数）越来越多，其计算得到的最优重力补偿数据的精确度也就越来越高，并稳定在重力补偿实际值附近。
由上可知，该机械臂重力补偿装置，通过获取机械臂的结构数据，根据结构数据，构建机械臂的D-H坐标系，基于D-H坐标系，确定机械臂中相邻两个关节间的齐次变换矩阵，根据齐次变换矩阵，结合各连杆的质量和质心位置，计算得到机械臂当前位姿所作用在每个关节上的重力补偿值，运用卡尔曼滤波方法，结合实时获取的机械臂在零速度模式下的重力补偿电流数据，对重力补偿值进行修正，得到机械臂当前位姿所作用在每个关节上的最优重力补偿数据；从而，通过卡尔曼滤波方法，结合机械臂在零速度模式下的重力补偿电流数据，对机械臂进行重力补偿，解决现有的机械臂重力补偿方法在机械臂负载的情况下会导致重力补偿出现偏差的问题，可以在负载或环境发生变化时对机械臂的重力补偿值进行实时调整，保证机械臂的控制稳定与运行平稳，使机械臂可以在高精度和高速度的工况下良好运行，提高了机械臂的重力补偿效率。
请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当电子设备运行时，处理器301执行该计算机程序，以执行上述实施例的任一可选的实现方式中的机械臂重力补偿方法，以实现以下功能：获取机械臂的结构数据，根据结构数据，构建机械臂的D-H坐标系，基于D-H坐标系，确定机械臂中相邻两个关节间的齐次变换矩阵，根据齐次变换矩阵，结合各连杆的质量和质心位置，计算得到机械臂当前位姿所作用在每个关节上的重力补偿值，运用卡尔曼滤波方法，结合实时获取的机械臂在零速度模式下的重力补偿电流数据，对重力补偿值进行修正，得到机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
本申请实施例提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的机械臂重力补偿方法，以实现以下功能：获取机械臂的结构数据，根据结构数据，构建机械臂的D-H坐标系，基于D-H坐标系，确定机械臂中相邻两个关节间的齐次变换矩阵，根据齐次变换矩阵，结合各连杆的质量和质心位置，计算得到机械臂当前位姿所作用在每个关节上的重力补偿值，运用卡尔曼滤波方法，结合实时获取的机械臂在零速度模式下的重力补偿电流数据，对重力补偿值进行修正，得到机械臂当前位姿所作用在每个关节上的最优重力补偿数据。其中，存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random Access Memory, 简称SRAM），电可擦除可编程只读存储器（Electrically Erasable Programmable Read-Only Memory, 简称EEPROM），可擦除可编程只读存储器（Erasable Programmable Read Only Memory, 简称EPROM），可编程只读存储器（Programmable Red-Only Memory, 简称PROM），只读存储器（Read-Only Memory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。
在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
另外，作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，既可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。
以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。
Claims (10)
1.一种机械臂重力补偿方法，用于对机械臂进行重力补偿，其特征在于，包括步骤：
获取机械臂的结构数据；
根据所述结构数据，构建所述机械臂的D-H坐标系；
基于所述D-H坐标系，确定所述机械臂中相邻两个关节间的齐次变换矩阵；
根据所述齐次变换矩阵，结合各连杆的质量和质心位置，计算得到所述机械臂当前位姿所作用在每个关节上的重力补偿值；
运用卡尔曼滤波方法，结合实时获取的所述机械臂在零速度模式下的重力补偿电流数据，对所述重力补偿值进行修正，得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
2.根据权利要求1所述的机械臂重力补偿方法，其特征在于，基于所述D-H坐标系，确定所述机械臂中相邻两个关节间的齐次变换矩阵，包括：
定义所述机械臂在所述D-H坐标系下的关节参数和连杆参数；
基于所述关节参数和所述连杆参数，确定所述机械臂中相邻两个关节间的齐次变换矩阵。
3.根据权利要求1所述的机械臂重力补偿方法，其特征在于根据所述齐次变换矩阵，结合各连杆的质量和质心位置，计算得到所述机械臂当前位姿所作用在每个关节上的重力补偿值，包括：
根据所述齐次变换矩阵、所述各连杆的质量和质心位置，计算所述机械臂中各连杆作用于各关节的重力力矩；
基于所述齐次变换矩阵和所述重力力矩，计算得到所述机械臂当前位姿所作用在每个关节上的重力补偿值。
4.根据权利要求1所述的机械臂重力补偿方法，其特征在于，运用卡尔曼滤波方法，结合实时获取的所述机械臂在零速度模式下的重力补偿电流数据，对所述重力补偿值进行修正，得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据，包括：
采用先标定后插值的方法，获取所述机械臂在零速度模式下的重力补偿电流数据；
根据预设的随机噪声向量，运用线性变换方式，将所述重力补偿电流数据转换为重力补偿向量；
基于所述重力补偿向量，结合所述重力补偿值的卡尔曼增益数据，计算得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
5.根据权利要求4所述的机械臂重力补偿方法，其特征在于，采用先标定后插值的方法，获取所述机械臂在零速度模式下的重力补偿电流数据，包括：
对所述机械臂中受重力影响的关节进行位置标定，得到所述机械臂的标定位置；
获取零速度模式下所述机械臂中标定位置的标定电流；
通过插值计算方法，根据所述标定电流，计算得到所述机械臂在零速度模式下的重力补偿电流数据。
6.根据权利要求4所述的机械臂重力补偿方法，其特征在于，基于所述重力补偿向量，结合所述重力补偿值的卡尔曼增益数据，计算得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据，包括：
以预设的重力误差阈值为依据，计算所述重力补偿值的卡尔曼增益数据；
基于所述卡尔曼增益数据，更新所述重力误差阈值，得到最优重力补偿方差；
根据所述最优重力补偿方差、所述重力补偿向量和所述卡尔曼增益数据，计算得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
7.根据权利要求6所述的机械臂重力补偿方法，其特征在于，基于所述卡尔曼增益数据，更新所述重力误差阈值，得到最优重力补偿方差之后，还包括：
将所述最优重力补偿方差设置为所述重力误差阈值。
8.一种机械臂重力补偿装置，用于对机械臂进行重力补偿，其特征在于，包括：
获取模块，用于获取机械臂的结构数据；
构建模块，用于根据所述结构数据，构建所述机械臂的D-H坐标系；
矩阵模块，用于基于所述D-H坐标系，确定所述机械臂中相邻两个关节间的齐次变换矩阵；
计算模块，用于根据所述齐次变换矩阵，结合各连杆的质量和质心位置，计算得到所述机械臂当前位姿所作用在每个关节上的重力补偿值；
修正模块，用于运用卡尔曼滤波方法，结合实时获取的所述机械臂在零速度模式下的重力补偿电流数据，对所述重力补偿值进行修正，得到所述机械臂当前位姿所作用在每个关节上的最优重力补偿数据。
9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如权利要求1-7任一项所述机械臂重力补偿方法中的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时运行如权利要求1-7任一项所述机械臂重力补偿方法中的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311189595.5A CN116922399B (zh) | 2023-09-15 | 2023-09-15 | 机械臂重力补偿方法、装置、电子设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311189595.5A CN116922399B (zh) | 2023-09-15 | 2023-09-15 | 机械臂重力补偿方法、装置、电子设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116922399A CN116922399A (zh) | 2023-10-24 |
| CN116922399B true CN116922399B (zh) | 2023-11-24 |
Family
ID=88377495
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311189595.5A Active CN116922399B (zh) | 2023-09-15 | 2023-09-15 | 机械臂重力补偿方法、装置、电子设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116922399B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108972626A (zh) * | 2018-08-22 | 2018-12-11 | 珠海格力电器股份有限公司 | 一种重力补偿方法、装置、存储介质及机器人 |
| CN109202889A (zh) * | 2017-06-30 | 2019-01-15 | 沈阳新松机器人自动化股份有限公司 | 一种柔性多关节机器人电流力控制系统及方法 |
| CN109773827A (zh) * | 2017-11-10 | 2019-05-21 | 上银科技股份有限公司 | 用于机械臂的负载估测重力补偿的方法及其系统 |
| WO2021238049A1 (zh) * | 2020-05-28 | 2021-12-02 | 杭州键嘉机器人有限公司 | 机械臂的多负载自适应重力补偿方法、装置及控制设备 |
| CN116392253A (zh) * | 2023-04-19 | 2023-07-07 | 上海卓昕医疗科技有限公司 | 一种应用于手术机器人的主动摆位方法及系统 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| TWI642523B (zh) * | 2017-09-21 | 2018-12-01 | 上銀科技股份有限公司 | 用於機械臂之負載估測重力補償的方法及其負載估測重力補償系統 |
-
2023
- 2023-09-15 CN CN202311189595.5A patent/CN116922399B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109202889A (zh) * | 2017-06-30 | 2019-01-15 | 沈阳新松机器人自动化股份有限公司 | 一种柔性多关节机器人电流力控制系统及方法 |
| CN109773827A (zh) * | 2017-11-10 | 2019-05-21 | 上银科技股份有限公司 | 用于机械臂的负载估测重力补偿的方法及其系统 |
| CN108972626A (zh) * | 2018-08-22 | 2018-12-11 | 珠海格力电器股份有限公司 | 一种重力补偿方法、装置、存储介质及机器人 |
| WO2021238049A1 (zh) * | 2020-05-28 | 2021-12-02 | 杭州键嘉机器人有限公司 | 机械臂的多负载自适应重力补偿方法、装置及控制设备 |
| CN116392253A (zh) * | 2023-04-19 | 2023-07-07 | 上海卓昕医疗科技有限公司 | 一种应用于手术机器人的主动摆位方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116922399A (zh) | 2023-10-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113066105B (zh) | 激光雷达和惯性测量单元融合的定位与建图方法及系统 | |
| CN113119100B (zh) | 冗余机械臂控制方法、装置、可读存储介质及设备 | |
| CN111168719B (zh) | 一种基于定位工装的机器人校准方法及系统 | |
| CN113459094B (zh) | 一种工业机器人工具坐标系及零点自标定方法 | |
| CN110053051B (zh) | 工业串联机器人关节刚度系数辨识方法 | |
| CN114523477B (zh) | 关节位姿的校准方法、系统及存储介质 | |
| CN110370271B (zh) | 工业串联机器人的关节传动比误差校准方法 | |
| CN113211445B (zh) | 一种机器人参数标定方法、装置、设备及存储介质 | |
| CN110561424A (zh) | 基于多传感器混合滤波器的在线机器人运动学校准方法 | |
| CN114216456B (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
| CN114474003A (zh) | 一种基于参数辨识的车载建筑机器人误差补偿方法 | |
| CN115008476B (zh) | 基于等效运动学模型的机器人标定方法及设备 | |
| TW202224872A (zh) | 機械手臂校正系統及機械手臂校正系統方法 | |
| CN112643658A (zh) | 基于sir降维dh模型的串联机器人自适应误差建模的标定方法 | |
| CN111687845A (zh) | 一种基于惯性测量单元的机械臂运动学参数标定方法 | |
| CN117428791B (zh) | 一种用于肩部四轴康复机器人的逆运动学求解方法及系统 | |
| CN116922399B (zh) | 机械臂重力补偿方法、装置、电子设备及存储介质 | |
| CN116901081B (zh) | 机器人dh参数标定方法、装置、电子设备及存储介质 | |
| CN113733098B (zh) | 机械臂模型位姿计算方法、装置、电子设备和存储介质 | |
| CN112847441B (zh) | 基于梯度下降法的六轴机器人坐标偏移检测方法和装置 | |
| CN117387834B (zh) | 一种力传感器标定方法、系统、电子设备和存储介质 | |
| CN114683259B (zh) | 机械手臂校正系统及机械手臂校正方法 | |
| CN117375471B (zh) | 永磁电机转动惯量与负载转矩辨识方法及系统 | |
| CN118544357A (zh) | 一种变载装置的机械臂非线性刚度补偿方法及系统 | |
| CN117744238A (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 |