CN116985136B - 基于四元数的机械臂节点姿态速度前瞻控制方法及装置 - Google Patents
基于四元数的机械臂节点姿态速度前瞻控制方法及装置 Download PDFInfo
- Publication number
- CN116985136B CN116985136B CN202311036088.8A CN202311036088A CN116985136B CN 116985136 B CN116985136 B CN 116985136B CN 202311036088 A CN202311036088 A CN 202311036088A CN 116985136 B CN116985136 B CN 116985136B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- speed
- arm node
- point
- track
- 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
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000004364 calculation method Methods 0.000 claims abstract description 28
- 230000001133 acceleration Effects 0.000 claims abstract description 17
- 230000008859 change Effects 0.000 claims abstract description 13
- 230000007704 transition Effects 0.000 claims abstract description 13
- 238000007781 pre-processing Methods 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 11
- 238000004590 computer program Methods 0.000 description 7
- 108010001267 Protein Subunits Proteins 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000009467 reduction Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification 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/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
- B25J18/00—Arms
-
- 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)
- Manipulator (AREA)
Abstract
本发明公开了一种基于四元数的机械臂节点姿态速度前瞻控制方法及装置，其中方法包括：在机械臂节点的移动路径上对若干个预设轨迹进行插补计算，得到机械臂节点在每一预设轨迹的起点后的路径规划；依据路径规划获取路径危险点，通过四元数相应调整机械臂节点姿态运动的进给速度，以实现机械臂节点姿态的平滑过渡。通过对机械臂节点预处理一段插补路径，自动分析轨迹路径，发现并处理路径危险点，对速度轨迹提前规划，实现速度的平滑过渡避免加减速频繁变化。
Description
技术领域
本发明涉及机械臂控制技术领域，特别涉及一种基于四元数的机械臂节点姿态速度前瞻控制方法及装置。
背景技术
机械臂在进行节点移动时，拐弯点是指机械臂路径上的转折点或转弯点。在规划机械臂的路径时，通常需要考虑拐弯点的选择，以便使机械臂运动平滑、快速且准确。
机械臂的欧拉角表示方法可能会遇到万向节锁死问题，这是由于欧拉角的定义和约束导致的。欧拉角是一种将机械臂的姿态表示为三个旋转角度(通常是绕固定坐标轴的旋转)，如滚转角、俯仰角和偏航角。在某些情况下，当机械臂的姿态转动过程中，特别是当俯仰角接近90度或-90度时，欧拉角存在万向节锁死的问题。这意味着，在特定的欧拉角组合下，无法唯一确定机械臂的姿态。
发明内容
本发明实施例的目的是提供一种基于四元数的机械臂节点姿态速度前瞻控制方法及装置，通过对机械臂节点预处理一段插补路径，自动分析轨迹路径，发现并处理路径危险点，对速度轨迹提前规划，实现速度的平滑过渡避免加减速频繁变化。
为解决上述技术问题，本发明实施例的第一方面提供了一种基于四元数的机械臂节点姿态速度前瞻控制方法，包括如下步骤：
在机械臂节点的移动路径上对预设轨迹进行插补计算，得到所述机械臂节点在每一所述预设轨迹的起点后的路径规划；
依据所述路径规划获取路径危险点，通过四元数相应调整所述机械臂节点姿态运动的进给速度，以实现所述机械臂节点姿态的平滑过渡。
进一步地，所述依据所述路径规划获取路径危险点相应调整所述机械臂节点的速度，包括：
基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度；
基于所述机械臂节点在所述预设轨迹的起点的实时速度，获取所述机械臂节点在所述预设轨迹的起点和所述路径危险点之间的速度调整起始点；
当所述机械臂节点移动至所述速度调整起始点时，控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度。
进一步地，所述基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度，包括：
当所述机械臂节点的移动路径为光滑轨迹时，通过四元数获取所述机械臂节点在所述路径规划中的曲率半径变化趋势，并计算所述安全进给速度，依据所述安全进给速度调整所述机械臂节点的进给速度。
进一步地，所述基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度，包括：
当所述机械臂节点的移动路径为非光滑轨迹时，基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角；
依据所述轨迹切线夹角计算所述机械臂节点在所述拐弯处的安全进给速度，并结合所述机械臂节点在所述预设轨迹的起点的实时速度相应调整其速度。
进一步地，所述机械臂节点在拐弯前后的轨迹切线夹角的计算公式为：
Vi＝min(v_front,v_behind,v_included_angle,v_nax) (2)
其中，quaternion_v_front为所述机械臂节点在拐弯前的前一轨迹在末端位置的四元数速度，quaternion_v_behind为所述机械臂节点在拐弯后的后一轨迹在末端位置的四元数速度，v_front轨迹点前段轨迹长度和初速度决定的最大速度，v_behind为轨迹点后段轨迹长度决定的最大速度，v_included_angle为前一轨迹和后一轨迹的夹角决定的最大速度，v_max为最大线速度，Vi为所述安全进给速度，i为轨迹端点数。
进一步地，所述基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角，包括：
计算每一姿态点的前一轨迹在所述姿态点的四元数速度和后一轨迹在所述姿态点的四元数速度；
计算两个所述四元数速度的夹角；
依据两个所述四元数速度的夹角，结合所述前一轨迹长度、所述后一轨迹长度及最大姿态角速度计算所述每一姿态点的姿态角速度。
进一步地，所述计算所述每一姿态点的姿态角速度之后，还包括：
判断当前所述姿态点是否为最后一个姿态点；
如否则计算后一个所述姿态点的姿态角速度；
如是判断当前所述姿态点是否受到后一个所述姿态点的速度限制，当由最后一个所述姿态点起，重新计算每一所述姿态点的姿态角速度，至第一个所述姿态点，以得到每一所述姿态点的最优姿态角速度。
进一步地，所述控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度，包括：
当所述机械臂节点在所述预设轨迹的起点的实时进给速度低于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第一预设加速度提高所述机械臂节点的进给速度；或，
当所述机械臂节点在所述预设轨迹的起点的实时进给速度高于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第二预设加速度降低所述机械臂节点的进给速度；或，
当所述机械臂节点在所述预设轨迹的起点的实时进给速度等于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后保持所述机械臂节点的进给速度。
相应地，本发明实施例的第二方面提供了一种基于四元数的机械臂节点姿态速度前瞻控制装置，包括：
插补计算模块，其用于在机械臂节点的移动路径上对预设轨迹进行插补计算，得到所述机械臂节点在每一所述预设轨迹的起点后的路径规划；
速度控制模块，其用于依据所述路径规划获取路径危险点，通过四元数相应调整所述机械臂节点姿态运动的进给速度，以实现所述机械臂节点姿态的平滑过渡。
进一步地，所述速度控制模块包括：
安全进给速度获取子模块，其用于基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度；
速度调整起始点获取控制子模块，其用于基于所述机械臂节点在所述预设轨迹的起点的实时速度，获取所述机械臂节点在所述预设轨迹的起点和所述路径危险点之间的速度调整起始点；
机械臂控制子模块，其用于当所述机械臂节点移动至所述速度调整起始点时，控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度。
进一步地，所述安全进给速度获取子模块包括：
第一安全进给速度获取单元，其用于当所述机械臂节点的移动路径为光滑轨迹时，通过四元数获取所述机械臂节点在所述路径规划中的曲率半径变化趋势，计算所述安全进给速度，依据所述安全进给速度调整所述机械臂节点的进给速度。
进一步地，所述安全进给速度获取子模块包括：
夹角计算单元，其用于当所述机械臂节点的移动路径为非光滑轨迹时，基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角；
速度调整单元，其用于依据所述轨迹切线夹角计算所述机械臂节点在所述拐弯处的安全进给速度，并结合所述机械臂节点在所述预设轨迹的起点的实时速度相应调整其速度。
进一步地，所述机械臂节点在拐弯前后的轨迹切线夹角的计算公式为：
Vi＝min(v_front，v_behind,v_included_angle，v_max) (2)
其中，quaternion_v_front为所述机械臂节点在拐弯前的前一轨迹在末端位置的四元数速度，quaternion_v_behind为所述机械臂节点在拐弯后的后一轨迹在末端位置的四元数速度，v_front轨迹点前段轨迹长度和初速度决定的最大速度，v_behind为轨迹点后段轨迹长度决定的最大速度，v_included_angle为前一轨迹和后一轨迹的夹角决定的最大速度，v_max为最大线速度，Vi为所述安全进给速度，i为轨迹端点数。
进一步地，所述夹角计算单元包括：
四元数速度计算子单元，其用于计算每一姿态点的前一轨迹在所述姿态点的四元数速度和后一轨迹在所述姿态点的四元数速度；
夹角计算子单元，其用于计算两个所述四元数速度的夹角；
姿态角速度计算子单元，其用于依据两个所述四元数速度的夹角，结合所述前一轨迹长度、所述后一轨迹长度及最大姿态角速度计算所述每一姿态点的姿态角速度。
进一步地，所述夹角计算单元还包括：
姿态点判断子单元，其用于判断当前所述姿态点是否为最后一个姿态点；
姿态点控制子单元，其用于在当前所述姿态点不为最后一个姿态点时，计算后一个所述姿态点的姿态角速度；
所述姿态点控制子单元还用于在当前所述姿态点为最后一个姿态点时，判断当前所述姿态点是否受到后一个所述姿态点的速度限制，当由最后一个所述姿态点起，重新计算每一所述姿态点的姿态角速度，至第一个所述姿态点，以得到每一所述姿态点的最优姿态角速度。
进一步地，所述机械臂控制子模块包括：
第一机械臂控制单元，其用于当所述机械臂节点在所述预设轨迹的起点的实时进给速度低于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第一预设加速度提高所述机械臂节点的进给速度；或，
第二机械臂控制单元，其用于当所述机械臂节点在所述预设轨迹的起点的实时进给速度高于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第二预设加速度降低所述机械臂节点的进给速度；或，
第三机械臂控制单元，其用于当所述机械臂节点在所述预设轨迹的起点的实时进给速度等于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后保持所述机械臂节点的进给速度。
相应地，本发明实施例的第三方面提供了一种电子设备，包括：至少一个处理器；以及与所述至少一个处理器连接的存储器；其中，所述存储器存储有可被所述一个处理器执行的指令，所述指令被所述一个处理器执行，以使所述至少一个处理器执行上述基于四元数的机械臂节点姿态速度前瞻控制方法。
本发明实施例的上述技术方案具有如下有益的技术效果：
通过对机械臂节点预处理一段插补路径，自动分析轨迹路径，发现并处理路径危险点，对速度轨迹提前规划，实现速度的平滑过渡避免加减速频繁变化。
附图说明
图1是本发明实施例提供的基于四元数的机械臂节点姿态速度前瞻控制方法流程图；
图2是本发明实施例提供的姿态角速度计算流程图；
图3是本发明实施例提供的基于四元数的机械臂节点姿态速度前瞻控制装置模块框图；
图4是本发明实施例提供的速度控制模块框图；
图5是本发明实施例提供的安全进给速度获取子模块框图；
图6是本发明实施例提供的夹角计算单元框图；
图7是本发明实施例提供的机械臂控制子模块框图。
附图标记：
1、插补计算模块，2、速度控制模块，21、安全进给速度获取子模块，211、第一安全进给速度获取单元，212、夹角计算单元，2121、四元数速度计算子单元，2122、夹角计算子单元，2123、姿态角速度计算子单元，2124、姿态点判断子单元，2125、姿态点控制子单元，213、速度调整单元，22、速度调整起始点获取控制子模块，23、机械臂控制子模块，231、第一机械臂控制单元，232、第二机械臂控制单元，233、第三机械臂控制单元。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。
请参照图1，本发明实施例的第一方面提供了一种基于四元数的机械臂节点姿态速度前瞻控制方法，包括如下步骤：
步骤S100，在机械臂节点的移动路径上对若干个预设轨迹进行插补计算，得到机械臂节点在每一预设轨迹的起点后的路径规划。
插补路径计算用于生成平滑和连续的路径，实现机器人在给定起点和终点之间的运动，并确定位置和姿态将路径参数化，即将路径的位置和姿态信息表示为时间或长度的函数。这可以通过离散化路径上的点，并为每个点分配一个时间或长度的值来实现。使用插值算法根据所选择的插补方法，在路径上生成连续的点。插值算法可以根据路径参数的变化情况，计算出路径上的中间点，并保证路径的平滑性和连续性。根据插值后的路径点，生成机器人控制的目标点。这些目标点可以在机器人控制系统中使用，实现机器人的精确运动。
步骤S300，依据路径规划获取路径危险点，通过四元数相应调整机械臂节点姿态运动的进给速度，以实现机械臂节点姿态的平滑过渡。
相对于欧拉角进行姿态控制时存在的万向节锁死问题，通过四元数对机械臂节点的姿态进行控制，可以完整地表示所有姿态，计算效率更高，姿态转换和控制的速度更快。四元数在旋转运动中更加稳定，不容易出现数值误差和累积误差；相比之下，欧拉角由于存在万向锁问题，容易出现数值计算不稳定性和误差积累。即，通过四元数对机械臂节点姿态进行控制可以提高计算效率、避免万向节锁死问题、实现平滑过渡，并提高数值稳定性。
进一步地，步骤S300中的依据路径规划获取路径危险点相应调整机械臂节点的速度，包括：
步骤S310，基于路径危险点，通过四元数获取与其相应的机械臂节点在路径危险点的安全进给速度。
步骤S320，基于机械臂节点在预设轨迹的起点的实时速度，获取机械臂节点在预设轨迹的起点和路径危险点之间的速度调整起始点。
步骤S330，当机械臂节点移动至速度调整起始点时，控制机械臂节点调整进给速度，并在路径危险点时调整至安全进给速度。
可选的，步骤S310中的基于路径危险点，通过四元数获取与其相应的机械臂节点在路径危险点的安全进给速度，包括：
步骤S311，当机械臂节点的移动路径为光滑轨迹时，通过四元数获取机械臂节点在路径规划中的曲率半径变化趋势，并计算安全进给速度，依据安全进给速度调整机械臂节点的进给速度。
具体的，对于光滑轨迹，进给速度主要由曲率半径的大小来控制，在曲率半径较小的区域,就要将速度降到一定范围内，而在曲率半径较大的区域，就要对其进行加速控制。因此在前瞻控制中就要通过预处理一段运动轨迹来分析其轨迹上的曲率半径变化趋势，然后根据曲率半径的变化趋势来决定速度的变化，确保速度的变化能满足精度的要求。然后根据加减速性能确定减速开始的点，保证运动加减速的平滑过渡。
可选的，步骤S310中的基于路径危险点，通过四元数获取与其相应的机械臂节点在路径危险点的安全进给速度，包括：
步骤S312a，当机械臂节点的移动路径为非光滑轨迹时，基于四元数获取机械臂节点在拐弯前后的轨迹切线夹角。
步骤S312b，依据轨迹切线夹角计算机械臂节点在拐弯处的安全进给速度，并结合机械臂节点在预设轨迹的起点的实时速度相应调整其速度。
具体的，对于非光滑轨迹，可将拐弯前后两段轨迹在拐弯处切线间夹角的大小作为减速特征。即可根据其取值确定拐弯处进给速度的允许值，然后根据速度的变化情况来决定是否进行减速处理，如果要进行减速处理则根据机械臂节点的加减速性能决定减速的起始点。
具体的，机械臂节点在拐弯前后的轨迹切线夹角的计算公式为：
Vi＝min(v_front,v_behind,v_included_angle,v_max) (2)
其中，quaternion_v_front为机械臂节点在拐弯前的前一轨迹在末端位置的四元数速度，quaternion_v_behind为机械臂节点在拐弯后的后一轨迹在末端位置的四元数速度，v_front轨迹点前段轨迹长度和初速度决定的最大速度，v_behind为轨迹点后段轨迹长度决定的最大速度，v_included_angle为前一轨迹和后一轨迹的夹角决定的最大速度，v_max为最大线速度，Vi为安全进给速度，i为轨迹端点数。
进一步地，请参照图2，步骤S312a中的基于四元数获取机械臂节点在拐弯前后的轨迹切线夹角，包括：
步骤S312a-1，计算每一姿态点的前一轨迹在姿态点的四元数速度和后一轨迹在姿态点的四元数速度。
步骤S312a-2，计算两个四元数速度的夹角。
步骤S312a-3，依据两个四元数速度的夹角，结合前一轨迹长度、后一轨迹长度及最大姿态角速度计算每一姿态点的姿态角速度。
进一步地，计算每一姿态点的姿态角速度之后，还包括：
步骤S312a-4，判断当前姿态点是否为最后一个姿态点。
步骤S312a-5，如否则计算后一个姿态点的姿态角速度。
步骤S312a-6，如是判断当前姿态点是否受到后一个姿态点的速度限制，当由最后一个姿态点起，重新计算每一姿态点的姿态角速度，至第一个姿态点，以得到每一姿态点的最优姿态角速度。
如图2所示，上述过程中对于姿态点Qi+1,先求出该姿态点前一段轨迹在该姿态点处的四元数速度，再求出后一段轨迹在该姿态点处的四元数速度。
根据公式1，求出两个四元数速度之间的夹角；再根据公式2，求出由前段轨迹长度、后段轨迹长度、夹角、最大姿态角速度决定的该姿态点的姿态角速度。重复上述操作，一直到最后一个点，即可求出各点位初步的姿态角速度。
由于前面各个点的姿态角速度，都是以后一段轨迹的末速度为0计算的，但实际运行过程中，后一段轨迹的末速度往往不为0，因此需要从最后一个点开始，重新计算各点位的姿态角速度，一直到第一个点结束。
综上，即可得到各衔接点处最优的衔接姿态角速度。
具体的，步骤S330中的控制机械臂节点调整进给速度，并在路径危险点时调整至安全进给速度，包括：
步骤S331，当机械臂节点在预设轨迹的起点的实时进给速度低于安全进给速度时，在机械臂节点到达速度调整起始点后按照第一预设加速度提高机械臂节点的进给速度。
或者，
步骤S332，当机械臂节点在预设轨迹的起点的实时进给速度高于安全进给速度时，在机械臂节点到达速度调整起始点后按照第二预设加速度降低机械臂节点的进给速度。
或者，
步骤S333，当机械臂节点在预设轨迹的起点的实时进给速度等于安全进给速度时，在机械臂节点到达速度调整起始点后保持机械臂节点的进给速度。
相应地，请参照图3，本发明实施例的第二方面提供了一种基于四元数的机械臂节点姿态速度前瞻控制装置，包括：
插补计算模块1，其用于在机械臂节点的移动路径上对若干个预设轨迹进行插补计算，得到机械臂节点在每一预设轨迹的起点后的路径规划；
速度控制模块2，其用于依据路径规划获取路径危险点，通过四元数相应调整机械臂节点姿态运动的进给速度，以实现机械臂节点姿态的平滑过渡。
进一步地，请参照图4，速度控制模块2包括：
安全进给速度获取子模块21，其用于基于路径危险点，通过四元数获取与其相应的机械臂节点在路径危险点的安全进给速度；
速度调整起始点获取控制子模块22，其用于基于机械臂节点在预设轨迹的起点的实时速度，获取机械臂节点在预设轨迹的起点和路径危险点之间的速度调整起始点；
机械臂控制子模块23，其用于当机械臂节点移动至速度调整起始点时，控制机械臂节点调整进给速度，并在路径危险点时调整至安全进给速度。
进一步地，请参照图5，安全进给速度获取子模块21包括：
第一安全进给速度获取单元211，其用于当机械臂节点的移动路径为光滑轨迹时，通过四元数获取机械臂节点在路径规划中的曲率半径变化趋势，计算安全进给速度，依据所述安全进给速度调整机械臂节点的进给速度。
进一步地，安全进给速度获取子模块21包括：
夹角计算单元212，其用于当机械臂节点的移动路径为非光滑轨迹时，基于四元数获取机械臂节点在拐弯前后的轨迹切线夹角；
速度调整单元213，其用于依据轨迹切线夹角计算机械臂节点在拐弯处的安全进给速度，并结合机械臂节点在预设轨迹的起点的实时速度相应调整其速度。
进一步地，机械臂节点在拐弯前后的轨迹切线夹角的计算公式为：
Vi＝min(v_front,v_behind,v_included_angle，v_max) (2)
其中，quaternion_v_front为机械臂节点在拐弯前的前一轨迹在末端位置的四元数速度，quaternion_v_behind为机械臂节点在拐弯后的后一轨迹在末端位置的四元数速度，v_front轨迹点前段轨迹长度和初速度决定的最大速度，v_behind为轨迹点后段轨迹长度决定的最大速度，v_included_angle为前一轨迹和后一轨迹的夹角决定的最大速度，v_max为最大线速度，Vi为安全进给速度，i为轨迹端点数。
进一步地，请参照图6，夹角计算单元212包括：
四元数速度计算子单元2121，其用于计算每一姿态点的前一轨迹在姿态点的四元数速度和后一轨迹在姿态点的四元数速度；
夹角计算子单元2122，其用于计算两个四元数速度的夹角；
姿态角速度计算子单元2123，其用于依据两个四元数速度的夹角，结合前一轨迹长度、后一轨迹长度及最大姿态角速度计算每一姿态点的姿态角速度。
进一步地，夹角计算单元212还包括：
姿态点判断子单元2124，其用于判断当前姿态点是否为最后一个姿态点；
姿态点控制子单元2125，其用于在当前姿态点不为最后一个姿态点时，计算后一个姿态点的姿态角速度；
姿态点控制子单元2125还用于在当前姿态点为最后一个姿态点时，判断当前姿态点是否受到后一个姿态点的速度限制，当由最后一个姿态点起，重新计算每一姿态点的姿态角速度，至第一个姿态点，以得到每一姿态点的最优姿态角速度。
进一步地，请参照图7，机械臂控制子模块23包括：
第一机械臂控制单元231，其用于当机械臂节点在预设轨迹的起点的实时进给速度低于安全进给速度时，在机械臂节点到达速度调整起始点后按照第一预设加速度提高机械臂节点的进给速度；或，
第二机械臂控制单元232，其用于当机械臂节点在预设轨迹的起点的实时进给速度高于安全进给速度时，在机械臂节点到达速度调整起始点后按照第二预设加速度降低机械臂节点的进给速度；或，
第三机械臂控制单元233，其用于当机械臂节点在预设轨迹的起点的实时进给速度等于安全进给速度时，在机械臂节点到达速度调整起始点后保持机械臂节点的进给速度。
相应地，本发明实施例的第三方面提供了一种电子设备，包括：至少一个处理器；以及与至少一个处理器连接的存储器；其中，存储器存储有可被一个处理器执行的指令，指令被一个处理器执行，以使至少一个处理器执行上述基于四元数的机械臂节点姿态速度前瞻控制方法。
本发明实施例旨在保护一种基于四元数的机械臂节点姿态速度前瞻控制方法及装置，其中方法包括：在机械臂节点的移动路径上对若干个预设轨迹进行插补计算，得到机械臂节点在每一预设轨迹的起点后的路径规划；依据路径规划获取路径危险点，通过四元数相应调整机械臂节点运动的进给速度，以实现机械臂节点的平滑过渡。上述技术方案具备如下效果：
通过对机械臂节点预处理一段插补路径，自动分析轨迹路径，发现并处理路径危险点，对速度轨迹提前规划，实现速度的平滑过渡避免加减速频繁变化。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。
Claims (7)
1.一种基于四元数的机械臂节点姿态速度前瞻控制方法，其特征在于，包括如下步骤：
在机械臂节点的移动路径上对若干个预设轨迹进行插补计算，得到所述机械臂节点在每一所述预设轨迹的起点后的路径规划；
依据所述路径规划获取路径危险点，通过四元数相应调整所述机械臂节点姿态运动的进给速度，以实现所述机械臂节点姿态的平滑过渡；
所述依据所述路径规划获取路径危险点，通过四元数相应调整所述机械臂节点姿态运动的进给速度，包括：
基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度；
基于所述机械臂节点在所述预设轨迹的起点的实时速度，获取所述机械臂节点在所述预设轨迹的起点和所述路径危险点之间的速度调整起始点；
当所述机械臂节点移动至所述速度调整起始点时，控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度；
所述基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度，包括：
当所述机械臂节点的移动路径为光滑轨迹时，通过四元数获取所述机械臂节点在所述路径规划中的曲率半径变化趋势，计算所述安全进给速度，依据所述安全进给速度调整所述机械臂节点的进给速度；
当所述机械臂节点的移动路径为非光滑轨迹时，基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角；
依据所述轨迹切线夹角计算所述机械臂节点在所述拐弯处的安全进给速度，并结合所述机械臂节点在所述预设轨迹的起点的实时速度相应调整其速度。
2.根据权利要求1所述的基于四元数的机械臂节点姿态速度前瞻控制方法，其特征在于，所述机械臂节点在拐弯前后的轨迹切线夹角的计算公式为：
Vi＝min(v_front,v_behind,v_included_angle,v_max) (2)
其中，quaternion_v_front为所述机械臂节点在拐弯前的前一轨迹在末端位置的四元数速度，quaternion_v_behind为所述机械臂节点在拐弯后的后一轨迹在初始位置的四元数速度，v_front为轨迹点前段轨迹长度和初速度决定的最大速度，v_behind为轨迹点后段轨迹长度决定的最大速度，v_included_angle为前一轨迹和后一轨迹的夹角决定的最大速度，v_max为最大线速度，Vi为所述安全进给速度，i为轨迹端点数。
3.根据权利要求1所述的基于四元数的机械臂节点姿态速度前瞻控制方法，其特征在于，所述基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角，包括：
计算每一姿态点的前一轨迹在所述姿态点的四元数速度和后一轨迹在所述姿态点的四元数速度；
计算两个所述四元数速度的夹角；
依据两个所述四元数速度的夹角，结合所述前一轨迹长度、所述后一轨迹长度及最大姿态角速度计算所述每一姿态点的姿态角速度。
4.根据权利要求3所述的基于四元数的机械臂节点姿态速度前瞻控制方法，其特征在于，所述计算所述每一姿态点的姿态角速度之后，还包括：
判断当前所述姿态点是否为最后一个姿态点；
如否则计算后一个所述姿态点的姿态角速度；
如是判断当前所述姿态点是否受到后一个所述姿态点的速度限制，当由最后一个所述姿态点起，重新计算每一所述姿态点的姿态角速度，至第一个所述姿态点，以得到每一所述姿态点的最优姿态角速度。
5.根据权利要求1-4任一所述的基于四元数的机械臂节点姿态速度前瞻控制方法，其特征在于，所述控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度，包括：
当所述机械臂节点在所述预设轨迹的起点的实时进给速度低于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第一预设加速度提高所述机械臂节点的进给速度；或，
当所述机械臂节点在所述预设轨迹的起点的实时进给速度高于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第二预设加速度降低所述机械臂节点的进给速度；或，
当所述机械臂节点在所述预设轨迹的起点的实时进给速度等于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后保持所述机械臂节点的进给速度。
6.一种基于四元数的机械臂节点姿态速度前瞻控制装置，其特征在于，包括：
插补计算模块，其用于在机械臂节点的移动路径上对预设轨迹进行插补计算，得到所述机械臂节点在每一所述预设轨迹的起点后的路径规划；
速度控制模块，其用于依据所述路径规划获取路径危险点，通过四元数相应调整所述机械臂节点运动的进给速度，以实现所述机械臂节点的平滑过渡；
所述速度控制模块包括：
安全进给速度获取子模块，其用于基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度；
速度调整起始点获取控制子模块，其用于基于所述机械臂节点在所述预设轨迹的起点的实时速度，获取所述机械臂节点在所述预设轨迹的起点和所述路径危险点之间的速度调整起始点；
机械臂控制子模块，其用于当所述机械臂节点移动至所述速度调整起始点时，控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度；
所述安全进给速度获取子模块包括：
第一安全进给速度获取单元，其用于当所述机械臂节点的移动路径为光滑轨迹时，通过四元数获取所述机械臂节点在所述路径规划中的曲率半径变化趋势，计算所述安全进给速度，依据所述安全进给速度调整所述机械臂节点的进给速度；
夹角计算单元，其用于当所述机械臂节点的移动路径为非光滑轨迹时，基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角；
速度调整单元，其用于依据所述轨迹切线夹角计算所述机械臂节点在所述拐弯处的安全进给速度，并结合所述机械臂节点在所述预设轨迹的起点的实时速度相应调整其速度。
7.一种电子设备，其特征在于，包括：至少一个处理器；以及与所述至少一个处理器连接的存储器；其中，所述存储器存储有可被所述一个处理器执行的指令，所述指令被所述一个处理器执行，以使所述至少一个处理器执行如权利要求1-5任一所述基于四元数的机械臂节点姿态速度前瞻控制方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311036088.8A CN116985136B (zh) | 2023-08-16 | 2023-08-16 | 基于四元数的机械臂节点姿态速度前瞻控制方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311036088.8A CN116985136B (zh) | 2023-08-16 | 2023-08-16 | 基于四元数的机械臂节点姿态速度前瞻控制方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116985136A CN116985136A (zh) | 2023-11-03 |
| CN116985136B true CN116985136B (zh) | 2024-02-09 |
Family
ID=88533844
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311036088.8A Active CN116985136B (zh) | 2023-08-16 | 2023-08-16 | 基于四元数的机械臂节点姿态速度前瞻控制方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116985136B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118456410A (zh) * | 2023-02-07 | 2024-08-09 | 中兴通讯股份有限公司 | 运动位姿同步规划方法、设备及介质 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020093253A1 (zh) * | 2018-11-06 | 2020-05-14 | 深圳配天智能技术研究院有限公司 | 机器人的运动控制方法、控制系统和存储装置 |
| CN115741695A (zh) * | 2022-11-21 | 2023-03-07 | 江南大学 | 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法 |
| CN115808904A (zh) * | 2022-12-01 | 2023-03-17 | 南京埃斯顿机器人工程有限公司 | 一种工业机器人圆弧过辅助点轨迹规划方法 |
| CN116117796A (zh) * | 2022-12-19 | 2023-05-16 | 广州数控设备有限公司 | 一种工业机器人姿态轨迹过渡与速度规划方法及系统 |
-
2023
- 2023-08-16 CN CN202311036088.8A patent/CN116985136B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020093253A1 (zh) * | 2018-11-06 | 2020-05-14 | 深圳配天智能技术研究院有限公司 | 机器人的运动控制方法、控制系统和存储装置 |
| CN115741695A (zh) * | 2022-11-21 | 2023-03-07 | 江南大学 | 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法 |
| CN115808904A (zh) * | 2022-12-01 | 2023-03-17 | 南京埃斯顿机器人工程有限公司 | 一种工业机器人圆弧过辅助点轨迹规划方法 |
| CN116117796A (zh) * | 2022-12-19 | 2023-05-16 | 广州数控设备有限公司 | 一种工业机器人姿态轨迹过渡与速度规划方法及系统 |
Non-Patent Citations (2)
| Title |
|---|
| 多路径段平滑过渡的自适应前瞻位姿插补算法;王斌锐 等;《控制与决策》;第34卷(第6期);第1211-1218页 * |
| 机械手空间圆弧位姿轨迹规划算法的实现;任秉银;梁兆东;孔民秀;;哈尔滨工业大学学报(第07期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116985136A (zh) | 2023-11-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108549328B (zh) | 自适应速度规划方法及系统 | |
| US8774965B2 (en) | Method and device for controlling a manipulator | |
| US11188056B2 (en) | Feedrate scheduling method for five-axis dual-spline curve interpolation | |
| US9764462B2 (en) | Robot apparatus and robot controlling method | |
| CN106393106B (zh) | 参数自适应密化的机器人nurbs曲线运动插补方法 | |
| CN109623166B (zh) | 一种激光切割的拐角处理方法及系统 | |
| CN110900612B (zh) | 一种位姿同步的六轴工业机器人轨迹平顺方法 | |
| CN116985136B (zh) | 基于四元数的机械臂节点姿态速度前瞻控制方法及装置 | |
| WO2018205276A1 (zh) | 运动轨迹平滑转接的方法和装置及其相关设备 | |
| CN112486101B (zh) | Nurbs曲线自适应前瞻插补方法 | |
| JP2010511919A (ja) | 許容差ベースの経路設計と制御の方法 | |
| WO2018019111A1 (zh) | 机器人及其关节运动控制方法和装置 | |
| CN113189938B (zh) | 一种连续加工路径的速度曲线规划方法 | |
| JP2009053926A (ja) | 経路計画装置及び経路計画方法 | |
| WO2018195689A1 (zh) | S型速度规划方法、装置、系统、机器人以及数控机床 | |
| CN111633668B (zh) | 一种用于机器人加工三维自由曲面的运动控制方法 | |
| CN111283683A (zh) | 一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法 | |
| CN114690767A (zh) | 一种机器人轨迹规划方法、系统以及机器人 | |
| US20210260761A1 (en) | Method And Control System For Controlling An Industrial Actuator | |
| CN114488941A (zh) | 微小线段的轨迹光顺方法、介质及机床数控设备 | |
| CN111405966B (zh) | 用于控制机器人组的方法和控制装置 | |
| JP4146549B2 (ja) | 軌跡制御方法及び軌跡制御方法を記録した記録媒体 | |
| CN113858213B (zh) | 一种用于目标同步的机器人动态轨迹规划方法 | |
| JP2003076410A (ja) | 速度制御装置、速度制御方法およびその方法をコンピュータに実行させるプログラム | |
| CN112883502A (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 |