CN116000925A - 机械臂跟踪方法、装置、设备及存储介质 - Google Patents
机械臂跟踪方法、装置、设备及存储介质 Download PDFInfo
- Publication number
- CN116000925A CN116000925A CN202211688602.1A CN202211688602A CN116000925A CN 116000925 A CN116000925 A CN 116000925A CN 202211688602 A CN202211688602 A CN 202211688602A CN 116000925 A CN116000925 A CN 116000925A
- Authority
- CN
- China
- Prior art keywords
- current
- gesture
- mechanical arm
- quaternion
- target
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 239000011159 matrix material Substances 0.000 claims description 55
- 238000010129 solution processing Methods 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000004590 computer program Methods 0.000 claims description 5
- 230000002159 abnormal effect Effects 0.000 abstract description 5
- 238000010586 diagram Methods 0.000 description 6
- 239000013598 vector Substances 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000004804 winding Methods 0.000 description 2
- 208000033748 Device issues Diseases 0.000 description 1
- 210000000988 bone and bone Anatomy 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000000802 evaporation-induced self-assembly Methods 0.000 description 1
- 210000002414 leg Anatomy 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 230000029058 respiratory gaseous exchange Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 210000002303 tibia Anatomy 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 210000000689 upper leg Anatomy 0.000 description 1
Images
Classifications
-
- 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
- Manipulator (AREA)
Abstract
本公开涉及一种机械臂跟踪方法、装置、设备及存储介质。在待控制机械臂的当前运动时刻，获取待控制机械臂对应的目标三维位置目标姿态四元数、当前三维位置和当前姿态四元数，在确定位置偏差大于或等于预设的第一偏差阈值，和/或，姿态偏差大于或等于预设的第二偏差阈值，则确定当前插补控制姿态和当前插补控制位置；计算待控制机械臂上各个关节的当前控制角度；最后下发当前控制角度进行跟踪。由此，采用四元数法表示姿态及姿态偏差，利用四元数良好的插值特性确定当前插补控制姿态以实时跟踪姿态，无论当前姿态四元数和目标姿态四元数的关系如何，可以连续稳定进行跟踪控制而不会出现异常路径或者随意路径，提高了机械臂跟踪的可靠性。
Description
技术领域
本公开涉及机械臂运动控制领域，尤其涉及一种机械臂跟踪方法、装置、设备及存储介质。
背景技术
在医疗机器人领域中，经常需要控制机器人的机械臂进行动态跟踪。例如，在关节置换手术中，切削股骨及胫骨时，用户腿部可能会有轻微晃动；在一些经皮穿刺手术中，由于用户的呼吸作用，经皮穿刺的操作区不可避免的会有周期性的运动。这些情况下，都需要机械臂具备跟踪控制的能力。
在机械臂的跟踪控制过程中，需要基于机械臂的实时位置和实时姿态，对机械臂进行跟踪控制。相关技术中，一般利用欧拉角表示机械臂的姿态，基于欧拉角表示的机械臂当前姿态和机械臂目标姿态计算姿态偏差，然后基于姿态偏差控制机械臂各个关节进行跟踪。然而，欧拉角表示姿态一般不具有唯一性，也就是说，不同的欧拉角可以表示同一个旋转方向，使得用欧拉角进行姿态插值出现万向节死锁问题，在很多情况下会出现随意的跟踪控制路径，基于随意的跟踪控制路径对机械臂进行跟踪控制时，容易出现机械臂跟踪异常等问题。
发明内容
为了解决上述技术问题，本公开提供了一种机械臂跟踪方法、装置、设备及存储介质。
第一方面，本公开提供了一种机械臂跟踪方法，该方法包括：
在待控制机械臂的当前运动时刻，获取所述待控制机械臂对应的目标状态数据和当前状态数据，其中，所述目标状态数据包括目标三维位置和目标姿态四元数，所述当前状态数据包括当前三维位置和当前姿态四元数；
若所述当前三维位置与所述目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，所述当前姿态四元数与所述目标姿
态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于所5述当前姿态四元数和所述目标姿态四元数确定当前插补控制姿态，以及基于所述当前三维位置和所述目标三维位置确定当前插补控制位置；
基于所述当前插补控制姿态和所述当前插补控制位置，计算所述待控制机械臂上各个关节的当前控制角度；
按照所述各个关节的当前控制角度控制所述待控制机械臂的各个0关节进行跟踪。
第二方面，本公开提供了一种机械臂跟踪装置，该装置包括：
获取模块，用于在待控制机械臂的当前运动时刻，获取所述待控制机械臂对应的目标状态数据和当前状态数据，其中，所述目标状态
数据包括目标三维位置和目标姿态四元数，所述当前状态数据包括当5前三维位置和当前姿态四元数；
确定模块，用于若所述当前三维位置与所述目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，所述当前姿态四元数与所述目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差
阈值，则基于所述当前姿态四元数和所述目标姿态四元数确定当前插0补控制姿态，以及基于所述当前三维位置和所述目标三维位置确定当
前插补控制位置；
计算模块，用于基于所述当前插补控制姿态和所述当前插补控制位置，计算所述待控制机械臂上各个关节的当前控制角度；
控制模块，用于按照所述各个关节的当前控制角度控制所述待控5制机械臂的各个关节进行跟踪。
第三方面，本公开实施例还提供了一种电子设备，该设备包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现第一方面所提供的方法。
第四方面，本公开实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现第一方面所提供的方法。
本公开实施例提供的技术方案与现有技术相比具有如下优点：
本公开实施例的一种机械臂跟踪方法、装置、设备及存储介质，在待控制机械臂的当前运动时刻，获取待控制机械臂对应的目标状态数据和当前状态数据，其中，目标状态数据包括目标三维位置和目标姿态四元数，当前状态数据包括当前三维位置和当前姿态四元数；若当前三维位置与目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，当前姿态四元数与目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于当前姿态四元数和目标姿态四元数确定当前插补控制姿态，以及基于当前三维位置和目标三维位置确定当前插补控制位置；基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的当前控制角度；按照各个关节的当前控制角度控制待控制机械臂的各个关节进行跟踪。由此，能够采用四元数法表示姿态及姿态偏差，利用四元数良好的插值特性确定当前插补控制姿态以实现姿态的实时跟踪，同时确定当前插补控制位置以实时跟踪位置，因此，无论当前姿态四元数和目标姿态四元数的关系如何，结合当前三维位置和目标三维位置，可以连续稳定进行跟踪控制而不会出现异常路径或者随意路径，提高了机械臂跟踪的可靠性。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。
为了更清楚地说明本公开实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本公开实施例提供的一种机械臂跟踪方法的流程示意图；
图2为本公开实施例提供的一种机械臂跟踪方法的逻辑示意图；
图3为本公开实施例提供的一种机械臂跟踪装置的结构示意图；
图4为本公开实施例提供的一种电子设备的结构示意图。
具体实施方式
为了能够更清楚地理解本公开的上述目的、特征和优点，下面将对本公开的方案进行进一步描述。需要说明的是，在不冲突的情况下，本公开的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本公开，但本公开还可以采用其他不同于在此描述的方式来实施；显然，说明书中的实施例只是本公开的一部分实施例，而不是全部的实施例。
在很多情况下，相关技术也会利用轴角或者旋转矩阵表示机械臂的姿态，基于轴角或者旋转矩阵表示机械臂当前姿态和机械臂目标姿态计算姿态偏差，然后，基于姿态偏差控制机械臂各个关节进行跟踪。
轴角是指用一个以矢量定义的旋转角，再加上一个标量定义的旋转角来表示旋转。通常的轴角表示为[x,y,z,theta]，x，y，z表示三个坐标轴，theta表示角度，此种方法表示非常直观，但也很紧凑。然而，利用轴角表示机械臂的姿态存在奇异问题，一些姿态下可能出现姿态路径不连续或者绕远的问题。
利用旋转矩阵表示机械臂的姿态的方式具有唯一性，但旋转矩阵采用九个数表示姿态，控制变量太多，因此不方便进行姿态插值，也无法保证姿态的连续性。
综上，采用相关技术中的几种方式表示机械臂的姿态时，每种方式存在其对应的一些弊端，有待改进。
为了解决上述问题，下面结合图1至图2对本公开实施例提供的机械臂跟踪方法进行说明。在本公开实施例中，该机械臂跟踪方法可以由电子设备执行。其中，电子设备可以包括平板电脑、台式计算机、笔记本电脑等具有通信功能的设备，也可以包括虚拟机或者模拟器模拟的设备。
图1示出了本公开实施例提供的一种机械臂跟踪方法的流程示意图。
如图1所示，该机械臂跟踪方法可以包括如下步骤。
S110、在待控制机械臂的当前运动时刻，获取待控制机械臂对应的目标状态数据和当前状态数据，其中，目标状态数据包括目标三维位置和目标姿态四元数，当前状态数据包括当前三维位置和当前姿态四元数。
在本实施例中，在关节置换或者经皮穿刺等场景下，电子设备需要控制医疗机器人的待控制机械臂进行跟踪，以精准定位待控制机械臂上各个关节的实时位置和实时姿态。
其中，待控制机械臂是指需要进行跟踪控制的机械臂，并且待控制机械臂包括多个关节。
其中，当前运动时刻是指当前跟踪时刻，随着时间推进，当前运动时刻不断变化。
其中，目标状态数据中的目标三维位置和目标姿态四元数是指标准的位置和标准的姿态。
其中，当前状态数据中的当前三维位置和当前姿态四元数是指实时的待调整位置和实时的待调整姿态。
可选地，目标三维位置和当前三维位置都可以用三维向量表示。目标三维位置可以用pd表示，当前三维位置可以用p表示。
可选地，目标姿态四元数和当前姿态四元数都可以用四维向量表示。目标姿态四元数可以用qd表示，当前姿态四元数可以用q表示。
S120、若当前三维位置与目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，当前姿态四元数与目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于当前姿态四元数和目标姿态四元数确定当前插补控制姿态，以及基于当前三维位置和目标三维位置确定当前插补控制位置。
可以理解的是，在当前运动时刻，电子设备需要确定待控制机械5臂的各个关节是否已经跟踪到目标。具体的，计算当前三维位置与目
标三维位置之间的位置偏差，以及计算当前姿态四元数与目标姿态四元数之间的姿态偏差，然后，判断位置偏差是否大于或等于预设的第一偏差阈值，以及判断姿态偏差是否大于或等于预设的第二偏差阈值，
若位置偏差大于或等于预设的第一偏差阈值，和/或，姿态偏差大于或0等于预设的第二偏差阈值，则说明待控制机械臂上至少有一个关节还
未跟踪到目标，需要控制待控制机械臂继续进行跟踪。
其中，预设的第一偏差阈值可以是位置偏差极小值，预设的第二偏差阈值可以是姿态偏差极小值。
进一步的，电子设备控制待控制机械臂进行跟踪时，需要对待控5制机械臂进行插补控制，具体是基于当前姿态四元数和目标姿态四元
数，计算当前插补控制姿态，以及基于当前三维位置和目标三维位置计算当前插补控制位置，使得基于当前插补控制位置逐步控制待控制机械臂的追踪位置，并基于当前插补控制姿态逐步控制待控制机械臂的追踪姿态。
0其中，当前插补控制姿态是指当前运动时刻下的单步姿态消除误
差，当前插补控制位置是指当前运动时刻下的单步位置消除误差。
S130、基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的当前控制角度。
在本实施例中，电子设备可以对当前插补控制姿态和当前插补控5制位置进行矩阵变换以及角度逆解，得到待控制机械臂上各个关节的
当前控制角度。
其中，当前控制角度是指待控制机械臂上各个关节的角度控制量。
S140、按照各个关节的当前控制角度控制待控制机械臂的各个关节进行跟踪。
可以理解的是，电子设备确定各个关节的当前控制角度之后，将各个关节的当前控制角度下发给待控制机械臂，使得待控制机械臂基于各个关节的当前控制角度，控制待控制机械臂的各个关节旋转，以实现实时跟踪的效果。
本公开实施例的一种机械臂跟踪方法，在待控制机械臂的当前运动时刻，获取待控制机械臂对应的目标状态数据和当前状态数据，其中，目标状态数据包括目标三维位置和目标姿态四元数，当前状态数据包括当前三维位置和当前姿态四元数；若当前三维位置与目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，当前姿态四元数与目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于当前姿态四元数和目标姿态四元数确定当前插补控制姿态，以及基于当前三维位置和目标三维位置确定当前插补控制位置；基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的当前控制角度；按照各个关节的当前控制角度控制待控制机械臂的各个关节进行跟踪。由此，能够采用四元数法表示姿态及姿态偏差，利用四元数良好的插值特性确定当前插补控制姿态以实现姿态的实时跟踪，同时确定当前插补控制位置以实时跟踪位置，因此，无论当前姿态四元数和目标姿态四元数的关系如何，结合当前三维位置和目标三维位置，可以连续稳定进行跟踪控制而不会出现异常路径或者随意路径，提高了机械臂跟踪的可靠性。
进一步的，在执行S140之后，还包括：
判断当前运动时刻是否为当前控制周期内的最后运动时刻；
若不是，则在待控制机械臂的下一运动时刻，获取待控制机械臂的对应的目标状态数据和下一状态数据，并重复上述步骤，直至下一运动时刻为当前控制周期内的最后运动时刻，结束控制待控制机械臂的各个关节进行跟踪。
其中，当前控制周期是指基于待控制机械臂的控制需求设置的控制周期。
其中，下一运动时刻的目标状态数据包括目标三维位置和目标姿态四元数，下一运动时刻的当前状态数据包括当前三维位置和当前姿态四元数。
具体的，对于下一运动时刻，计算当前三维位置与目标三维位置之间的位置偏差，以及计算当前姿态四元数与目标姿态四元数之间的姿态偏差；然后，判断位置偏差是否大于或等于预设的第一偏差阈值，以及判断姿态偏差是否大于或等于预设的第二偏差阈值，若位置偏差大于或等于预设的第一偏差阈值，和/或，姿态偏差大于或等于预设的第二偏差阈值，则说明待控制机械臂上至少有一个关节还未跟踪到目标，需要控制待控制机械臂继续进行跟踪；接着，基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的当前控制角度；按照各个关节的当前控制角度控制待控制机械臂的各个关节进行跟踪；最后，判断下一运动时刻是否为当前控制周期内的最后运动时刻，若是，则结束控制待控制机械臂的各个关节进行跟踪。
由此，可以实现在整个控制周期的每个时刻，对待控制机械臂的各个关节的姿态和位置进行连续跟踪。
在本公开另一种实施方式中，在上述实施例的基础上，还可以进行最短姿态路径优化，并且，提供计算量较小，且适用于实时跟踪的插补方式，另外，细化了当前控制角度的计算方式。
在本公开一些实施例中，对姿态路径进行了优化。
可以理解的是，在四维空间中，单位球面分为上下两个半球，每个半球上的点都和三维空间的姿态有一一映射关系，所以三维空间姿态和单位四元数之间是一对二的映射关系，同一个三维空间姿态对应的两个单位四元数可以规划出不同的姿态路径。其中一条姿态路径是比较近的路径，另一条姿态路径是绕远的路径。
为了避免规划出绕远路径，在执行S110之后，该方法还包括：计算目标姿态四元数与当前姿态四元数的点积；若点积小于0，则将当前姿态四元数的相反数替换为当前姿态四元数。相应的，S120中的当前姿态四元数是替换后的数据。
可以理解的是，如果点积的结果是负值(夹角大于90°)，那么得到的当前插补控制姿态对应的就会是四维球面上的绕远姿态路径，此时需要将两个四元数的其中一个取反，具体可以将当前姿态四元数取反。
可选地，点积可以表示为：dot(qd，q)，若dot(qd，q)小于0，则q＝-q。
可选地，S120中的位置偏差可以通过如下方式表示：
pe＝||pd-p||
其中，pe是位置偏差。
可选地，S120中的姿态偏差可以通过如下方式表示：
qe＝arccos(dot(qd，q))，0<＝qe<＝Π
其中，qe是姿态偏差。
由此，计算目标姿态四元数与当前姿态四元数的点积，并在点积小于零时，对当前姿态四元数取反，可以保证规划出的姿态路径是最短路径，因此避免出现姿态路径绕远的问题。
在另一些实施例中，可以提供一种适用于计算量较小，且适用于实时跟踪的插补方式。
在本公开实施例中，可选地，S120具体包括：
获取预先设置的单步插补系数；
基于当前姿态四元数、目标姿态四元数以及预先设置的单步插补系数，计算当前插补控制姿态；
基于当前三维位置、目标三维位置以及预先设置的单步插补系数，计算当前插补控制位置。
其中，单步插补系数可以理解为单步跟踪比例系数。
可选地，当前插补控制姿态可以通过如下方式表示：
qc＝slerp(q，qd，c)
其中，c是单步插补系数，qc是当前插补控制姿态。
可选地，当前插补控制位置可以通过如下方式表示：
pc＝p+(pd-p)*c
其中，pc是当前插补控制位置。
由此，采用姿态和位置的同步插补方式，只需要一个单步插补系数，可以实现对姿态和位置进行同步跟踪，并且，采用Slerp四元数球面插补算法，计算量小，能够进行实时跟踪。
在本公开又一些实施例中，细化了当前控制角度的计算方式。
在本公开实施例中，可选的，S130具体包括：
S1301、基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的状态齐次矩阵；
S1302、对待控制机械臂上各个关节的状态齐次矩阵进行逆解处理，得到待控制机械臂上各个关节的当前控制角度。
其中，S1301具体包括：基于预先获取的姿态与旋转矩阵之间的转换关系，将当前插补控制姿态转换为旋转矩阵；将旋转矩阵与当前插补控制位置对应的位置矩阵进行组合，生成待控制机械臂上各个关节的状态齐次矩阵。
具体的，可以基于上述转换关系，将当前插补控制姿态qc转换为3*3的旋转矩阵，并且，当前插补控制位置对应的位置矩阵pc为3*1的矩阵，然后，将3*3的旋转矩阵与3*1的位置矩阵进行组合，得到4*4的状态齐次矩阵Tc。
其中，S1302具体包括：判断状态齐次矩阵所在的当前坐标系是否为机械臂基座坐标系；若不是，则基于当前坐标系与机械臂基座坐标系之间的转换关系，将状态齐次矩阵由当前坐标系转换到机械臂基座坐标系；对机械臂基座坐标系下的状态齐次矩阵进行逆解处理，得到待控制机械臂上各个关节的当前控制角度。
其中，当前坐标系可以是参考坐标系，例如是跟骨头固连的坐标系。
具体的，在确定状态齐次矩阵所在的当前坐标系不是机械臂基座坐标系的情况下，则基于上述转换关系，将状态齐次矩阵由当前坐标系转换到机械臂基座坐标系，并基于逆运动学算法，对机械臂基座坐标系下的状态齐次矩阵Tc进行逆解处理，得到待控制机械臂上各个关节的当前控制角度。
由此，可以先将当前插补控制姿态转换为旋转矩阵，并与当前插补控制位置对应的位置矩阵进行组合，得到待控制机械臂上各个关节的状态齐次矩阵，然后将机械臂基座坐标系下的状态齐次矩阵进行逆解处理，准确的计算出待控制机械臂上各个关节的当前控制角度。
在本公开又一种实施方式中，为了便于理解，提供了一种机械臂跟踪的逻辑示意图。
如图2所示，该机械臂跟踪方法可以包括如下步骤。
S210、获取单步插补系数c。
S220、获取目标三维位置pd、目标姿态四元数qd、当前三维位置p以及当前姿态四元数q。
S230、计算目标姿态四元数qd与当前姿态四元数q的点积dot(qd，q)，若点积dot(qd，q)小于0，则将当前姿态四元数取反，q＝-q。
S240、计算当前三维位置p与目标三维位置pd之间的位置偏差pe＝||pd-p||，以及计算当前姿态四元数q与目标姿态四元数qd之间的姿态偏差qe＝arccos(dot(qd，q))。
S250、判断位置偏差是否小于预设的第一偏差阈值，以及判断姿态偏差是否小于预设的第二偏差阈值。
其中，若位置偏差大于或等于预设的第一偏差阈值，和/或，姿态偏差大于或等于预设的第二偏差阈值，则执行S260，否则，结束。
S260、计算当前插补控制姿态qc＝slerp(q，qd，c)，以及计算当前插补控制位置pc＝p+(pd-p)*c。
S270、将当前插补控制姿态qc转换为旋转矩阵，并与当前插补控制位置对应的位置矩阵组合，生成状态齐次矩阵Tc。
S280、将状态齐次矩阵Tc转换到机械臂基座坐标系。
S290、对状态齐次矩阵Tc进行逆解处理，得到待控制机械臂上各个关节的当前控制角度。
S291、将各个关节的当前控制角度下发至待控制机械臂，以使待控制机械臂的各个关节基于当前控制角度进行跟踪。
在本实施例中，执行完S291之后，返回执行S220。
需要说明的是，S210～S291的具体实现方式可以参见前述实施例的描述，在此不做赘述。
本公开实施例还提供了一种用于实现上述的机械臂跟踪方法的机械臂跟踪装置，下面结合图3进行说明。在本公开实施例中，该机械臂跟踪装置可以为电子设备。其中，电子设备可以包括平板电脑、台式计算机、笔记本电脑等具有通信功能的设备，也可以包括虚拟机或者模拟器模拟的设备。
图3示出了本公开实施例提供的一种机械臂跟踪装置的结构示意图。
如图3所示，机械臂跟踪装置300可以包括：
获取模块310，用于在待控制机械臂的当前运动时刻，获取待控制机械臂对应的目标状态数据和当前状态数据，其中，目标状态数据包括目标三维位置和目标姿态四元数，当前状态数据包括当前三维位置和当前姿态四元数；
确定模块320，用于若当前三维位置与目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，当前姿态四元数与目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于当前姿态四元数和目标姿态四元数确定当前插补控制姿态，以及基于当前三维位置和目标三维位置确定当前插补控制位置；
计算模块330，用于基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的当前控制角度；
控制模块340，用于按照各个关节的当前控制角度控制待控制机械臂的各个关节进行跟踪。
本公开实施例的一种机械臂跟踪装置，在待控制机械臂的当前运动时刻，获取待控制机械臂对应的目标状态数据和当前状态数据，其中，目标状态数据包括目标三维位置和目标姿态四元数，当前状态数据包括当前三维位置和当前姿态四元数；若当前三维位置与目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，当前姿态四元数与目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于当前姿态四元数和目标姿态四元数确定当前插补控制姿态，以及基于当前三维位置和目标三维位置确定当前插补控制位置；基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的当前控制角度；按照各个关节的当前控制角度控制待控制机械臂的各个关节进行跟踪。由此，能够采用四元数法表示姿态及姿态偏差，利用四元数良好的插值特性确定当前插补控制姿态以实现姿态的实时跟踪，同时确定当前插补控制位置以实时跟踪位置，因此，无论当前姿态四元数和目标姿态四元数的关系如何，结合当前三维位置和目标三维位置，可以连续稳定进行跟踪控制而不会出现异常路径或者随意路径，提高了机械臂跟踪的可靠性。
在本公开一些实施例中，该装置还包括：
点积计算模块，用于计算目标姿态四元数与当前姿态四元数的点积；
替换模块，用于若点积小于0，则将当前姿态四元数的相反数替换为当前姿态四元数。
在本公开一些实施例中，确定模块320具体用于：
获取预先设置的单步插补系数；
基于当前姿态四元数、目标姿态四元数以及预先设置的单步插补系数，计算当前插补控制姿态；
基于当前三维位置、目标三维位置以及预先设置的单步插补系数，计算当前插补控制位置。
在本公开一些实施例中，计算模块330具体用于：
基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的状态齐次矩阵；
对待控制机械臂上各个关节的状态齐次矩阵进行逆解处理，得到待控制机械臂上各个关节的当前控制角度。
在本公开一些实施例中，计算模块330具体用于：
基于预先获取的姿态与旋转矩阵之间的转换关系，将当前插补控制姿态转换为旋转矩阵；
将旋转矩阵与当前插补控制位置对应的位置矩阵进行组合，生成待控制机械臂上各个关节的状态齐次矩阵。
在本公开一些实施例中，计算模块330用于：
判断状态齐次矩阵所在的当前坐标系是否为机械臂基座坐标系；
若不是，则基于当前坐标系与机械臂基座坐标系之间的转换关系，将状态齐次矩阵由当前坐标系转换到机械臂基座坐标系；
对机械臂基座坐标系下的状态齐次矩阵进行逆解处理，得到待控制机械臂上各个关节的当前控制角度。
在本公开一些实施例中，该装置还包括：
判断模块，用于判断当前运动时刻是否为当前控制周期内的最后运动时刻；
继续跟踪模块，用于若不是，则在待控制机械臂的下一运动时刻，获取待控制机械臂的对应的目标状态数据和下一状态数据，并重复上述步骤，直至下一运动时刻为当前控制周期内的最后运动时刻，结束控制待控制机械臂的各个关节进行跟踪。
需要说明的是，图3所示的机械臂跟踪装置300可以执行图1至图2所示的方法实施例中的各个步骤，并且实现图1至图2所示的方法实施例中的各个过程和效果，在此不做赘述。
图4示出了本公开实施例提供的一种电子设备的结构示意图。
如图4所示，该电子设备可以包括处理器401以及存储有计算机程序指令的存储器402。
具体地，上述处理器401可以包括中央处理器(CPU)，或者特定集成电路(Application Specific Integrated Circuit，ASIC)，或者可以被配置成实施本申请实施例的一个或多个集成电路。
存储器402可以包括用于信息或指令的大容量存储器。举例来说而非限制，存储器402可以包括硬盘驱动器(Hard Disk Drive，HDD)、软盘驱动器、闪存、光盘、磁光盘、磁带或通用串行总线(Universal Serial Bus，USB)驱动器或者两个及其以上这些的组合。在合适的情况下，存储器402可包括可移除或不可移除(或固定)的介质。在合适的情况下，存储器402可在综合网关设备的内部或外部。在特定实施例中，存储器402是非易失性固态存储器。在特定实施例中，存储器402包括只读存储器(Read-Only Memory，ROM)。在合适的情况下，该ROM可以是掩模编程的ROM、可编程ROM(Programmable ROM，PROM)、可擦除PROM(Electrical Programmable ROM，EPROM)、电可擦除PROM(Electrically ErasableProgrammable ROM，EEPROM)、电可改写ROM(Electrically Alterable ROM，EAROM)或闪存，或者两个或及其以上这些的组合。
处理器401通过读取并执行存储器402中存储的计算机程序指令，以执行本公开实施例所提供的机械臂跟踪方法的步骤。
在一个示例中，该电子设备还可包括收发器403和总线404。其中，如图4所示，处理器401、存储器402和收发器403通过总线404连接并完成相互间的通信。
总线404包括硬件、软件或两者。举例来说而非限制，总线可包括加速图形端口(Accelerated Graphics Port，AGP)或其他图形总线、增强工业标准架构(ExtendedIndustry Standard Architecture，EISA)总线、前端总线(Front Side BUS，FSB)、超传输(Hyper Transport，HT)互连、工业标准架构(Industrial Standard Architecture，ISA)总线、无限带宽互连、低引脚数(Low Pin Count，LPC)总线、存储器总线、微信道架构(MicroChannel Architecture，MCA)总线、外围控件互连(Peripheral Component Interconnect，PCI)总线、PCI-Express(PCI-X)总线、串行高级技术附件(Serial Advanced TechnologyAttachment，SATA)总线、视频电子标准协会局部(Video Electronics StandardsAssociation Local Bus，VLB)总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线404可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线，但本申请考虑任何合适的总线或互连。
以下是本公开实施例提供的计算机可读存储介质的实施例，该计算机可读存储介质与上述各实施例的机械臂跟踪方法属于同一个发明构思，在计算机可读存储介质的实施例中未详尽描述的细节内容，可以参考上述机械臂跟踪方法的实施例。
本实施例提供一种包含计算机可执行指令的存储介质，计算机可执行指令在由计算机处理器执行时用于执行一种机械臂跟踪方法，该方法包括：
在待控制机械臂的当前运动时刻，获取待控制机械臂对应的目标状态数据和当前状态数据，其中，目标状态数据包括目标三维位置和目标姿态四元数，当前状态数据包括当前三维位置和当前姿态四元数；
若当前三维位置与目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，当前姿态四元数与目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于当前姿态四元数和目标姿态四元数确定当前插补控制姿态，以及基于当前三维位置和目标三维位置确定当前插补控制位置；
基于当前插补控制姿态和当前插补控制位置，计算待控制机械臂上各个关节的当前控制角度；
按照各个关节的当前控制角度控制待控制机械臂的各个关节进行跟踪。
当然，本公开实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上的方法操作，还可以执行本公开任意实施例所提供的机械臂跟踪方法中的相关操作。
通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本公开可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本公开的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器(Read-Only Memory,ROM)、随机存取存储器(RandomAccess Memory,RAM)、闪存(FLASH)、硬盘或光盘等，包括若干指令用以使得一台计算机云平台(可以是个人计算机，服务器，或者网络云平台等)执行本公开各个实施例所提供的机械臂跟踪方法。
注意，上述仅为本公开的较佳实施例及所运用技术原理。本领域技术人员会理解，本公开不限于这里的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本公开的保护范围。因此，虽然通过以上实施例对本公开进行了较为详细的说明，但是本公开不仅仅限于以上实施例，在不脱离本公开构思的情况下，还可以包括更多其他等效实施例，而本公开的范围由所附的权利要求范围决定。
Claims (10)
1.一种机械臂跟踪方法，其特征在于，包括：
在待控制机械臂的当前运动时刻，获取所述待控制机械臂对应的目标状态数据和当前状态数据，其中，所述目标状态数据包括目标三维位置和目标姿态四元数，所述当前状态数据包括当前三维位置和当前姿态四元数；
若所述当前三维位置与所述目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，所述当前姿态四元数与所述目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于所述当前姿态四元数和所述目标姿态四元数确定当前插补控制姿态，以及基于所述当前三维位置和所述目标三维位置确定当前插补控制位置；
基于所述当前插补控制姿态和所述当前插补控制位置，计算所述待控制机械臂上各个关节的当前控制角度；
按照所述各个关节的当前控制角度控制所述待控制机械臂的各个关节进行跟踪。
2.根据权利要求1所述的方法，其特征在于，在所述获取所述待控制机械臂对应的目标状态数据和当前状态数据之后，所述方法还包括：
计算所述目标姿态四元数与所述当前姿态四元数的点积；
若所述点积小于0，则将所述当前姿态四元数的相反数替换为所述当前姿态四元数。
3.根据权利要求1所述的方法，其特征在于，所述基于所述当前姿态四元数和所述目标姿态四元数确定当前插补控制姿态，以及基于所述当前三维位置和所述目标三维位置确定当前插补控制位置，包括：
获取预先设置的单步插补系数；
基于所述当前姿态四元数、所述目标姿态四元数以及所述预先设置的单步插补系数，计算所述当前插补控制姿态；
基于所述当前三维位置、所述目标三维位置以及所述预先设置的单步插补系数，计算所述当前插补控制位置。
4.根据权利要求1所述的方法，其特征在于，所述基于所述当前插补控制姿态和所述当前插补控制位置，计算所述待控制机械臂上各个关节的当前控制角度，包括：
基于所述当前插补控制姿态和所述当前插补控制位置，计算所述待控制机械臂上各个关节的状态齐次矩阵；
对所述待控制机械臂上各个关节的状态齐次矩阵进行逆解处理，得到所述待控制机械臂上各个关节的当前控制角度。
5.根据权利要求4所述的方法，其特征在于，所述基于所述当前插补控制姿态和所述当前插补控制位置，计算所述待控制机械臂上各个关节的状态齐次矩阵，包括：
基于预先获取的姿态与旋转矩阵之间的转换关系，将所述当前插补控制姿态转换为旋转矩阵；
将所述旋转矩阵与所述当前插补控制位置对应的位置矩阵进行组合，生成所述待控制机械臂上各个关节的状态齐次矩阵。
6.根据权利要求4所述的方法，其特征在于，所述对所述待控制机械臂上各个关节的状态齐次矩阵进行逆解处理，得到所述待控制机械臂上各个关节的当前控制角度，包括：
判断所述状态齐次矩阵所在的当前坐标系是否为机械臂基座坐标系；
若不是，则基于所述当前坐标系与所述机械臂基座坐标系之间的转换关系，将所述状态齐次矩阵由所述当前坐标系转换到所述机械臂基座坐标系；
对所述机械臂基座坐标系下的状态齐次矩阵进行逆解处理，得到所述待控制机械臂上各个关节的当前控制角度。
7.根据权利要求1所述的方法，其特征在于，还包括：
判断所述当前运动时刻是否为当前控制周期内的最后运动时刻；
若不是，则在所述待控制机械臂的下一运动时刻，获取所述待控制机械臂的对应的目标状态数据和下一状态数据，并重复上述步骤，直至所述下一运动时刻为所述当前控制周期内的最后运动时刻，结束控制待控制机械臂的各个关节进行跟踪。
8.一种机械臂跟踪控制装置，其特征在于，包括：
获取模块，用于在待控制机械臂的当前运动时刻，获取所述待控制机械臂对应的目标状态数据和当前状态数据，其中，所述目标状态数据包括目标三维位置和目标姿态四元数，所述当前状态数据包括当前三维位置和当前姿态四元数；
确定模块，用于若所述当前三维位置与所述目标三维位置之间的位置偏差大于或等于预设的第一偏差阈值，和/或，所述当前姿态四元数与所述目标姿态四元数之间的姿态偏差大于或等于预设的第二偏差阈值，则基于所述当前姿态四元数和所述目标姿态四元数确定当前插补控制姿态，以及基于所述当前三维位置和所述目标三维位置确定当前插补控制位置；
计算模块，用于基于所述当前插补控制姿态和所述当前插补控制位置，计算所述待控制机械臂上各个关节的当前控制角度；
控制模块，用于按照所述各个关节的当前控制角度控制所述待控制机械臂的各个关节进行跟踪。
9.一种电子设备，其特征在于，包括：
处理器；
存储器，用于存储可执行指令；
其中，所述处理器用于从所述存储器中读取所述可执行指令，并执行所述可执行指令以实现上述权利要求1-7中任一项所述的方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述存储介质存储有计算机程序，当所述计算机程序被处理器执行时，使得处理器实现上述权利要求1-7中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211688602.1A CN116000925A (zh) | 2022-12-27 | 2022-12-27 | 机械臂跟踪方法、装置、设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211688602.1A CN116000925A (zh) | 2022-12-27 | 2022-12-27 | 机械臂跟踪方法、装置、设备及存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116000925A true CN116000925A (zh) | 2023-04-25 |
Family
ID=86020515
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211688602.1A Pending CN116000925A (zh) | 2022-12-27 | 2022-12-27 | 机械臂跟踪方法、装置、设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116000925A (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020093253A1 (zh) * | 2018-11-06 | 2020-05-14 | 深圳配天智能技术研究院有限公司 | 机器人的运动控制方法、控制系统和存储装置 |
| CN111775146A (zh) * | 2020-06-08 | 2020-10-16 | 南京航空航天大学 | 一种工业机械臂多工位作业下的视觉对准方法 |
| WO2021134644A1 (zh) * | 2019-12-31 | 2021-07-08 | 深圳市大疆创新科技有限公司 | 云台的控制方法和云台 |
| CN114571452A (zh) * | 2022-03-01 | 2022-06-03 | 湖南科技职业学院 | 工业机器人轨迹规划方法、电子设备及可读存储介质 |
| CN115122330A (zh) * | 2022-07-14 | 2022-09-30 | 合肥智行通智能科技有限公司 | 一种基于多通道遥控器的机械臂控制方法 |
-
2022
- 2022-12-27 CN CN202211688602.1A patent/CN116000925A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020093253A1 (zh) * | 2018-11-06 | 2020-05-14 | 深圳配天智能技术研究院有限公司 | 机器人的运动控制方法、控制系统和存储装置 |
| WO2021134644A1 (zh) * | 2019-12-31 | 2021-07-08 | 深圳市大疆创新科技有限公司 | 云台的控制方法和云台 |
| CN111775146A (zh) * | 2020-06-08 | 2020-10-16 | 南京航空航天大学 | 一种工业机械臂多工位作业下的视觉对准方法 |
| CN114571452A (zh) * | 2022-03-01 | 2022-06-03 | 湖南科技职业学院 | 工业机器人轨迹规划方法、电子设备及可读存储介质 |
| CN115122330A (zh) * | 2022-07-14 | 2022-09-30 | 合肥智行通智能科技有限公司 | 一种基于多通道遥控器的机械臂控制方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108874119B (zh) | 跟踪臂移动以生成计算机系统的输入的系统和方法 | |
| US20200338730A1 (en) | Trajectory planning device, trajectory planning method and program | |
| US11498220B2 (en) | Control system and control method | |
| JP2021103564A (ja) | 仮想オブジェクト駆動方法、装置、電子機器及び可読記憶媒体 | |
| CN109719725B (zh) | 一种机械臂运动规划的方法、机械臂及机器人 | |
| US11605192B2 (en) | Skeleton model update apparatus, skeleton model update method, and program | |
| CN108927801B (zh) | 一种机械臂末端姿态调整方法及装置 | |
| CN112022355A (zh) | 基于计算机视觉的手眼标定方法及装置、存储介质 | |
| CN112603542B (zh) | 手眼标定方法、装置、电子设备和存储介质 | |
| US11845006B2 (en) | Skeleton model updating apparatus, skeleton model updating method, and program | |
| WO2023168849A1 (zh) | 机械臂动作捕捉方法、介质、电子设备及系统 | |
| WO2024037658A1 (zh) | 机器人指向动作控制方法、装置、电子设备和存储介质 | |
| CN113696188A (zh) | 手眼标定数据采集方法、装置、电子设备及存储介质 | |
| CN113119104B (zh) | 机械臂控制方法、机械臂控制装置、计算设备及系统 | |
| CN113524201B (zh) | 机械臂位姿主动调节方法、装置、机械臂和可读存储介质 | |
| WO2018119642A1 (zh) | 一种工业机器人的工具坐标系原点的标定方法及装置 | |
| CN116000925A (zh) | 机械臂跟踪方法、装置、设备及存储介质 | |
| CN115919250A (zh) | 一种人体动态关节角测量系统 | |
| US11763464B2 (en) | Estimation apparatus, learning apparatus, estimation method, learning method, and program | |
| CN111113428B (zh) | 机器人控制方法、机器人控制装置及终端设备 | |
| CN115670660A (zh) | 标定方法、装置、手术机器人、电子设备及存储介质 | |
| US20220015844A1 (en) | Kinematics of wristed laparoscopic instruments | |
| CN113450903A (zh) | 一种人体动作映射方法、装置、计算机设备和存储介质 | |
| CN115869069A (zh) | 手术机器人控制方法、装置、设备、介质及系统 | |
| US20210390750A1 (en) | Skeleton model update apparatus, skeleton model update method, and program |
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 |