WO2021243945A1 - 一种对于静止或低速工况下机械臂高精度力反馈的方法、机械臂辅助的外科手术方法和具有处理器可执行的非易失的程序代码的计算机可读介质 - Google Patents

一种对于静止或低速工况下机械臂高精度力反馈的方法、机械臂辅助的外科手术方法和具有处理器可执行的非易失的程序代码的计算机可读介质 Download PDF

Info

Publication number
WO2021243945A1
WO2021243945A1 PCT/CN2020/125842 CN2020125842W WO2021243945A1 WO 2021243945 A1 WO2021243945 A1 WO 2021243945A1 CN 2020125842 W CN2020125842 W CN 2020125842W WO 2021243945 A1 WO2021243945 A1 WO 2021243945A1
Authority
WO
WIPO (PCT)
Prior art keywords
robotic arm
joint
force feedback
matrix
coordinate system
Prior art date
Application number
PCT/CN2020/125842
Other languages
English (en)
French (fr)
Inventor
甘博涵
许靖
乔天
文理为
杜思傲
董旭亮
荣健
Original Assignee
杭州键嘉机器人有限公司
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 杭州键嘉机器人有限公司 filed Critical 杭州键嘉机器人有限公司
Publication of WO2021243945A1 publication Critical patent/WO2021243945A1/zh

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • 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/1607—Calculation of inertia, jacobian matrixes and inverses

Definitions

  • the present disclosure relates to the technical field of robotics, and in particular, to a method for high-precision force feedback of a mechanical arm under stationary or low-speed working conditions and a system for surgical operation assisted by the mechanical arm.
  • the current mainstream method is to first mathematically model the robot arm body.
  • Such mathematical modeling usually needs to identify the mass, inertia, center of mass position and other parameters of each segment of the manipulator, and then apply the identified parameters to the calculation of force feedback.
  • traditional methods usually perform parameter identification on robotic arms, such as 3-axis and 7-axis robotic arms.
  • the identified parameters include mass, center of mass, inertia and friction.
  • the traditional method takes up a lot of resources in calculation, and the calculation cost is very high.
  • the present disclosure provides a method for high-precision force feedback of a mechanical arm under stationary or low-speed conditions, including the following steps:
  • S500 Substitute the data of S400 into S300, and calculate the result
  • the present disclosure provides a surgical operation method assisted by a robotic arm, including the use of a robotic arm to perform surgery on a subject, wherein the method for high-precision force feedback of the robotic arm under stationary or low-speed conditions described in the present disclosure is used to perform the robotic arm Force feedback.
  • the present disclosure also provides a computer-readable medium having non-volatile program code executable by a processor, wherein the program code causes the processor to execute the robot arm described in the present disclosure for static or low-speed operating conditions. High-precision force feedback method.
  • Fig. 1 is a flowchart of a method of the present disclosure for high-precision force feedback of a mechanical arm under stationary or low-speed conditions.
  • Fig. 2 is a schematic diagram of the joint coordinate system of the method of the present disclosure for high-precision force feedback of a manipulator under stationary or low-speed conditions.
  • Fig. 3 is a schematic diagram of the centroid coordinate system of the method of the present disclosure for high-precision force feedback of a manipulator under stationary or low-speed conditions.
  • the present disclosure provides a method for high-precision force feedback of a mechanical arm under stationary or low-speed conditions, which includes the following steps:
  • S200 Construct a coordinate system whose origin is at the centroid position of each segment of the robotic arm based on the aforementioned joint coordinate system, and calculate the Jacobian matrix of the centroid position;
  • S500 Substitute the data of S400 into S300, and calculate the result
  • the coordinate system of each joint of the robotic arm is constructed according to a combination of translation and Euler angles.
  • parameter separation is performed, and the parameters related to joint positions are put into one matrix, and the parameters related to mass and centroid are combined and put into another matrix.
  • the robotic arm in the S400 step, it is ensured that the robotic arm is in a stationary state during sampling and the sampling points cover the entire working space.
  • the parameter related to the joint position includes a joint angle.
  • the parameters related to mass and center of mass include mass, position and length of the center of mass.
  • step S400 the data at the singular point position where the robot arm lacks some or all degrees of freedom is removed.
  • the robotic arm is a multi-axis robotic arm.
  • the present disclosure provides a surgical operation method assisted by a robotic arm, including the use of a robotic arm to perform surgery on a subject, wherein the method for high-precision force feedback of the robotic arm under stationary or low-speed conditions described in the present disclosure is used to perform the robotic arm Force feedback.
  • the robotic arm is a multi-axis robotic arm.
  • the present disclosure provides a system for a surgical operation assisted by a robotic arm, the system including:
  • a non-transitory storage medium storing processor-executable instructions
  • processor-executable instructions cause the processor to execute the method according to any one of claims 1-8.
  • the robotic arm is a multi-axis robotic arm.
  • the present disclosure provides a computer-readable medium with non-volatile program code executable by a processor, wherein the program code causes the processor to execute the robot arm height described in the present disclosure for static or low-speed operating conditions. Precision force feedback method.
  • the robotic arm includes a 2-axis robotic arm, a 3-axis robotic arm, a 4-axis robotic arm, a 5-axis robotic arm, a 6-axis robotic arm, a 7-axis robotic arm, an 8-axis robotic arm, 9-axis robotic arm, 10-axis robotic arm, 11-axis robotic arm, 12-axis robotic arm, 13-axis robotic arm, 14-axis robotic arm, 15-axis robotic arm, 16-axis robotic arm, 17-axis robotic arm, 18-axis robotic arm, 19-axis robotic arm, 20-axis robotic arm, 21-axis robotic arm, 22-axis robotic arm, 23-axis robotic arm, 24-axis robotic arm, 25-axis robotic arm, 26-axis robotic arm, 27-axis robotic arm, 28-axis robotic arm, 29-axis robotic arm or 30-axis robotic arm.
  • the complete dynamic equation should include the inertial term B(q), centrifugal force and Coriolis force term
  • Q in the equation Respectively indicate the position, velocity and acceleration of the joint.
  • the ⁇ on the right side of the equation represents the moment of each joint. After this simplification, only the relationship between the gravity term and the joint torque is left in the equation. This simplification can greatly reduce the calculation amount of a single sampling point, allow more sampling points to be added to the calculation, and improve the identification accuracy of the entire system under specific working conditions.
  • the joint coordinate system constructed by the present disclosure follows the coordinate axis z axis parallel to the joint axis and conforms to the right-hand rule. Please refer to Figure 1.
  • the coordinate system is constructed by combining translation and Euler angles.
  • the required parameters are different from those required by the traditional D-H method. The required parameters are listed in Table 1:
  • Table 1 Parameter table of joint coordinate system construction
  • ⁇ x is the rotation angle of the x-axis
  • ⁇ y is the rotation angle of the y-axis.
  • the transformation angle of the z-axis is the angle that the joint has rotated, so it is not included in the parameter table.
  • the parameter table only shows the conversion relationship between the x and y coordinate axes.
  • coordinate system 2 first performs translation transformation on the basis of coordinate system 1, and the moving distance is d 12 . Then substitute the angle that the joint 1 has rotated into the angle that the z-axis should rotate, and then rotate the x and y axes in turn. The rotation angle corresponds to the angle shown in the corresponding column in Table 1.
  • the coordinate system construction of the remaining joints is based on the previous joint coordinate system.
  • the Jacobian matrix in the forward kinematics of the manipulator is responsible for converting the motion speed in the joint space to the motion speed in the Cartesian coordinate system, namely:
  • v e , ⁇ e respectively represents the Cartesian coordinate system speed of the end effector, the translation component of the speed, and the rotation component of the speed.
  • the Jacobian matrix J in the right half is related to the current joint angle, ⁇ i and Respectively indicate the angle and angular velocity that the i-th joint has turned.
  • the first three rows of the Jacobian matrix correspond to the amount of translation in the Cartesian coordinate system, and the last three rows correspond to the amount of rotation.
  • the total number of columns should be equal to the number of joints of the robotic arm.
  • the gravity term in the dynamic equation contains a large number of unknown parameters, including the mass (m), the position of the center of mass (cm) and the length (d) of each segment.
  • Centroid cm cm 1 , cm 2 , cm 3 ,..., cm n
  • the gravity term G can be split into two items, as shown below:
  • the matrix Y is only related to the known joint angle ⁇ . Therefore, for a specific pose, only the matrix U is unknown in the equation, and it can be solved by solving a linear equation. However, due to the large number of unknowns and the potential connection between the unknowns, if they are solved directly, the internal connection will be ignored, which will lead to the solution of the linear equation falling into the predicament of local optimality. Therefore, it is necessary to recombine the unknowns, fully display the internal mathematical connections, and reduce the number of parameters that need to be identified.
  • the reorganization parameters are as follows:
  • Table 2 Identification parameter reorganization table
  • sampling points when collecting sampling points, it is necessary to ensure that the sampling points are distributed as much as possible in the entire working space, so as to improve the identification accuracy.
  • the working space of each joint can be uniformly sampled to obtain random joint positions.
  • the sampling moment should ensure that the robotic arm is basically in a static state, collect the joint position and joint torque information at the current moment, and then move to the next position. Singular point positions may be encountered during the movement. Because the robot arm will lose some degrees of freedom at the singular point positions, the sampling points near the singular points need to be removed from the data set.
  • the least square method can be used to solve the above formula and the matrix u can be solved, that is, all the parameters related to the kinetic model are identified.
  • step (1) the parameters have been identified, and step (2) will complete the measurement of the external force on the end effector based on the identified parameters.
  • the parameter matrix u is known, and the matrix Y is related to the joint angle during measurement.
  • the obtained matrix ⁇ int represents the torque provided to each joint by the weight of the manipulator body.
  • the external force has the following relationship between the joint space and the Cartesian space:
  • F ext contains the force and moment values in the Cartesian coordinate system, so it is a six-dimensional vector.
  • the external force F ext expressed on the end effector based on the Cartesian coordinate system can be obtained.
  • the reading of the joint torque sensor is used as the input of the identification program. Compared with the existing scheme of measuring the current of the motor to estimate the joint torque, the identification accuracy can be effectively improved.
  • the traditional method has a wider range of application, and can better estimate the force even at high speeds and large acceleration changes.
  • the disadvantage is that the amount of calculation is relatively large and the calculation cost is relatively high.
  • the method of the present disclosure is improved on the basis of the traditional method.
  • the inventor analyzed the particularity of the working conditions of the medical manipulator.
  • the method has been simplified by eliminating the particularity of the operating conditions of the medical manipulator obtained from the analysis.
  • the present disclosure limits the method to force feedback under static or low-speed conditions, which greatly reduces the number of parameters that need to be identified, reduces the requirements for calculation capabilities, and makes the force feedback under such working conditions more accurate.
  • the method of the present disclosure can be used in a variety of surgical operations assisted by robotic arms, including orthopedic surgery, tumor resection surgery, laparoscopic surgery, thoracoscopic surgery, etc., and can also be applied to medical examinations, including B-ultrasound and radiological examinations.
  • This disclosure analyzes the particularity of the working conditions of the medical manipulator.
  • the method of the present disclosure is suitable for force feedback under static or low-speed conditions.
  • the joint velocity and acceleration in the dynamic equation are close to zero under this condition, and only the gravity term is retained to adapt to the static or low-speed conditions to obtain dynamics.
  • the method of the present disclosure greatly reduces the number of parameters that need to be identified, reduces the requirement for calculation ability, and makes the force feedback under such working conditions more accurate.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本公开公开了一种对于静止或低速工况下机械臂高精度力反馈的方法、一种机械臂辅助的外科手术方法和具有处理器可执行的非易失的程序代码的计算机可读介质，该方法包括以下步骤：S100，构建机械臂的各个关节坐标系，并计算关节的雅可比矩阵；S200，根据上述的关节坐标系，构建原点位于机械臂的每一段质心位置的坐标系，并计算质心位置的雅可比矩阵；S300，构建动力学方程中重力项的矩阵；S400，在机械臂运动过程中采集得到关节位置和力矩数据；S500，将S400的数据代入到S300中，计算出结果；S600，得到施加外力导致的额外关节扭矩；S700，结合S100中得到的关节雅可比矩阵和S600中得到的额外关节扭矩，计算得到机械臂末端外力的大小和方向。本公开提供的一种对于静止或低速工况下机械臂高精度力反馈的方法，简化计算步骤，提高力反馈精确性。

Description

一种对于静止或低速工况下机械臂高精度力反馈的方法、机械臂辅助的外科手术方法和具有处理器可执行的非易失的程序代码的计算机可读介质
相关申请的交叉引用
本申请要求于2020年6月3日提交中国专利局的申请号为202010494425.8、名称为“一种对于静止或低速工况下机械臂高精度力反馈的方法”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本公开涉及机器人技术领域，具体地说，涉及一种对于静止或低速工况下机械臂高精度力反馈的方法和用于机械臂辅助的外科手术的系统。
背景技术
近年来，工业机械臂在医疗领域的应用正在快速发展中。在机械臂的帮助下，原本难度高、精度差、用时长的手术，得到了不同程度的改善。协作型机械臂作为工业机械臂的一个分支，凭借其人机协作性好、不用设置安全防护等优势，获得了医疗行业的青睐。不同于在工业上应用的机械臂，医疗机械臂更加注重安全性而非效率，因此医疗机械臂的运动基本是维持在静止或者缓慢移动的状态。然而为保证操作的灵活性和安全性，医疗机械臂对外力的感知要求会比对工业场景下的机械臂要求更高。如何提升机械臂力反馈的精度，是很多外科医生关心的问题。
目前主流的方法是先对机械臂本体进行数学建模。这样的数学建模通常需要辨识机械臂每段的质量、惯性量、质心位置等参数，然后再将辨识的参数应用于力反馈的计算。例如，传统的方法通常对机械臂，例如3轴和7轴机械臂，进行了参数辨识。辨识的参数包括质量、质心、惯性量和摩擦力。传统的方法在计算上需要占用大量资源，运算成本很高。
发明内容
本公开的目的包括例如提供一种对于静止或低速工况下机械臂高精度力反馈的方法，简化计算步骤，提高力反馈精确性。
本公开提供一种对于静止或低速工况下机械臂高精度力反馈的方法，包括以下步骤：
S100，构建机械臂的各个关节坐标系，并计算关节的雅可比矩阵；
S200，根据上述的关节坐标系，构建原点位于机械臂的每一段质心位置的坐标系，并计算质心位置的雅可比矩阵；
S300，构建动力学方程中重力项的矩阵；
S400，设置激励运动轨迹，在机械臂运动过程中采集得到关节位置和力矩数据；
S500，将S400的数据代入到S300中，计算出结果；
S600，将辨识的参数代入到动力学方程中，得到机械臂在特定位姿下由于本体产生的关节扭矩，将计算所得与实时测量的关节扭矩相减，得到施加外力导致的额外关节扭矩；
S700，结合S100中得到的关节雅可比矩阵和S600中得到的额外关节扭矩，计算得到机械臂末端外力的大小和方向。
本公开提供一种机械臂辅助的外科手术方法，包括使用机械臂对受试者进行手术，其中，使用本公开所述的对于静止或低速工况下机械臂高精度力反馈的方法进行机械臂的力反馈。
本公开还提供一种具有处理器可执行的非易失的程序代码的计算机可读介质，其中，所述程序代码使所述处理器执行本公开所述的对于静止或低速工况下机械臂高精度力反馈方法。
本公开的对于静止或低速工况下机械臂高精度力反馈的方法的有益效果至少包括：对医疗机械臂工况的特殊性进行了分析，本公开的方法适用于静止或低速条件下的力反馈，在动力学方程中的关节速度和加速度在该种条件下接近于零，仅保留重力项以适应静止或低速的工况，得到动力学方程中重力项的矩阵，大大减少了需要辨识的参数数量，降低了对计算能力的要求，并且使在此种工况下的力反馈更加精确。
附图说明
为了更清楚地说明本公开实施方式的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本公开的某些实施方式，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1是本公开对于静止或低速工况下机械臂高精度力反馈的方法的流程图。
图2是本公开对于静止或低速工况下机械臂高精度力反馈的方法的关节坐标系示意图。
图3是本公开对于静止或低速工况下机械臂高精度力反馈的方法的质心坐标系示意图。
具体实施方式
为使本公开实施方式的目的、技术方案和优点更加清楚，下面结合具体实施例和说明书附图对本公开做进一步阐述和说明。
本公开提供一种对于静止或低速工况下机械臂高精度力反馈的方法，其中，包括以下步骤：
S100，构建机械臂的各个关节坐标系，并计算关节的雅可比矩阵；
S200，根据上述的关节坐标系，构建原点位于机械臂的每一段质心位置的坐标系，并计算质心位置的雅可比矩阵；
S300，构建动力学方程中重力项的矩阵；
S400，设置激励运动轨迹，在机械臂运动过程中采集得到关节位置和力矩数据；
S500，将S400的数据代入到S300中，计算出结果；
S600，将辨识的参数代入到动力学方程中，得到机械臂在特定位姿下由于本体产生的关节扭矩，将计算所得与实时测量的关节扭矩相减，得到施加外力导致的额外关节扭矩；
S700，结合S100中得到的关节雅可比矩阵和S600中得到的额外关节扭矩，计算得到机械臂末端外力的大小和方向。
在一种或多种实施方式中，所述S100步骤中，根据平移和欧拉角结合的方式构建机械臂各个关节坐标系。
在一种或多种实施方式中，所述S300步骤中，进行参数分离，将与关节位置有关的参数放入一个矩阵，将与质量和质心有关的参数组合后放入另一个矩阵。
在一种或多种实施方式中，所述S400步骤中，采样时保证机械臂处于静止状态，且采样点布满整个工作空间。
在一种或多种实施方式中，所述与关节位置有关的参数包括关节角。
在一种或多种实施方式中，所述与质量和质心有关的参数包括质量、质心位置和长度。
在一种或多种实施方式中，所述S400步骤中，去除机械臂缺失部分或全部自由度的奇异点位置处的数据。
在一种或多种实施方式中，所述机械臂为多轴机械臂。
本公开提供一种机械臂辅助的外科手术方法，包括使用机械臂对受试者进行手术，其中，使用本公开所述的对于静止或低速工况下机械臂高精度力反馈的方法进行机械臂的力反馈。
在一种或多种实施方式中，所述机械臂为多轴机械臂。
本公开提供一种用于机械臂辅助的外科手术的系统，所述系统包括：
机械臂；
处理器；以及
存储有处理器可执行指令的非暂时性存储介质，
其中，所述处理器可执行指令使所述处理器执行根据权利要求1-8中任一项所述的方法。
在一种或多种实施方式中，所述机械臂为多轴机械臂。
本公开提供一种具有处理器可执行的非易失的程序代码的计算机可读介质，其中，所述程序代码使所述处理器执行本公开所述的对于静止或低速工况下机械臂高精度力反馈方法。
在一种或多种实施方式中，所述机械臂包括2轴机械臂、3轴机械臂、4轴机械臂、5轴机械臂、6轴机械臂、7轴机械臂、8轴机械臂、9轴机械臂、10轴机械臂、11轴机械臂、12轴机械臂、13轴机械臂、14轴机械臂、15轴机械臂、16轴机械臂、17轴机械臂、18轴机械臂、19轴机械臂、20轴机械臂、21轴机械臂、22轴机械臂、23轴机械臂、24轴机械臂、25轴机械臂、26轴机械臂、27轴机械臂、28轴机械臂、29轴机械臂或30轴机械臂。
请参考图1，为更具体解释步骤中包含的技术细节，下面将结合附图以及在KUKA LBR Med 7 R800七轴协作型机械臂上的实际操作对本公开进行进一步解释。
完整的动力学方程应当包含惯性项B(q)，离心力与科氏力项
Figure PCTCN2020125842-appb-000001
重力项G(q)，粘滞摩擦力项F v和库伦摩擦力项F c。方程中的q,
Figure PCTCN2020125842-appb-000002
分别表示关节的位置、速度和加速度。发明人经过多次试验，令人惊讶地发现当工况处于静止或低速运动状态下，关节速度
Figure PCTCN2020125842-appb-000003
和加速度
Figure PCTCN2020125842-appb-000004
接近于0，仅保留重力项以适应静止或低速的工况，因此动力学方程可以做如下简化：
Figure PCTCN2020125842-appb-000005
方程右边的τ表示的是各个关节上的力矩。经过这样的简化，方程中只剩下重力项和关节力矩的关系。该简化可以极大减少单个采样点的运算量，允许更多采样点加入运算，提高整个系统在特定工况下的辨识精度。
以KUKA LBR Med 7 R800为例，本公开构建的关节坐标系遵循坐标轴z轴平行于关节轴线且符合右手定则。请参考图一，使用平移和欧拉角结合的方式构建坐标系，所需要的参数与传统的D-H法构建所需参数有所不同，所需参数如表1所列：
表1：关节坐标系构建参数表
  关节1-2 关节2-3 关节3-4 关节4-5 关节5-6 关节6-7
α x π/2 π/2 π/2 π/2 π/2 π/2
α y π π 0 π 0 π
其中α x为x轴的旋转角度；α y为y轴的旋转角度。
由于z轴与关节轴同向，因此z轴的变换角度即是关节转动过的角度，因此未被列入参数表内。该参数表只表示的是x和y坐标轴之间的转换关系。
以关节1和关节2的转换关系为例，坐标系2在坐标系1的基础上先进行平移变换，移动的距离为d 12。然后将关节1转过的角度代入z轴应转动的角度，再依次旋转x和y轴， 转动角度对应表1中对应列所示角度。其余各关节的坐标系构建均基于前一关节坐标系进行构建。
使用公式可表示为：
Figure PCTCN2020125842-appb-000006
由此公式，可以计算得到各关节坐标系相对上一坐标系的转换矩阵：
Figure PCTCN2020125842-appb-000007
Figure PCTCN2020125842-appb-000008
Figure PCTCN2020125842-appb-000009
在实际运用中，基坐标系和末端执行器坐标系往往发挥重要作用，这两个坐标系关于关节坐标系的转换关系如下：
Figure PCTCN2020125842-appb-000010
将上文所述所有转换关系进行整合，即可得到任意关节或末端执行器坐标系相对于基坐标系的转换关系：
Figure PCTCN2020125842-appb-000011
雅可比矩阵在机械臂正运动学中承担了将关节空间的运动速度转换到笛卡尔坐标系中的运动速度，即：
Figure PCTCN2020125842-appb-000012
其中v e、
Figure PCTCN2020125842-appb-000013
ω e分别表示末端执行器的笛卡尔坐标系速度、速度的平移分量和速度的旋转分量。右半部分的雅可比矩阵J与当前关节角有关，θ i和
Figure PCTCN2020125842-appb-000014
分别表示第i关节转过的角度和角速度。
雅可比矩阵的求解可以通过如下方法求偏导得到：
Figure PCTCN2020125842-appb-000015
雅可比矩阵的前三行与笛卡尔坐标系下的平移量相对应，后三行与旋转量对应，总列数应当等于机械臂的关节个数。
在构建机械臂的数学模型时，由于机械臂的每一段都是刚体，因此可以将每一段刚体的质量集中在各段的质心处，再用合适的变换关系将各质心处的坐标系串联起来，构成简化的机械臂质量分布模型，从而用来分析机械臂的动力学特征。在以KUKA LBR Med 7 R800为基础的说明中，规定了第i段的机械臂质心坐标系旋转矩阵与i+1段保持一致，平移量是未知的，先使用符号cm表示。采用与上述相同的方法，可以建立如附图2所示的机械臂质心坐标系，以及计算出对应的雅可比矩阵。
动力学方程中的重力项中含有大量未知参数，包括每一段的质量(m)、质心位置(cm)和长度(d)。
质量m：m 1，m 2，m 3，……，m n
质心cm：cm 1，cm 2，cm 3，……，cm n
长度d：d 12，d 23，d 34，……，d (n-1)n
为了将未知参数辨识出来，可将重力项G拆分成两项，如下所示：
Figure PCTCN2020125842-appb-000016
拆分后矩阵Y只与已知的关节角θ相关，因此对于特定位姿，方程中仅有矩阵U未知，可以用解线性方程的方式来求解。然而由于未知量数量大，且未知量之间又存在潜在联系，若直接求解，则忽略了其内在联系，会导致线性方程的解陷入局部最优的困境。因此需要对未知量进行重新组合，将内部数学联系充分展示，降低需要辨识的参数个数。重组参数如下所示：
表2：辨识参数重组表
Figure PCTCN2020125842-appb-000017
Figure PCTCN2020125842-appb-000018
在重组的参数表中，只有对机械臂的动力学建模会产生影响的基础参数得到了保留。经过参数重组后，将不再需要辨识原先的大量参数，而只需要辨识如上所列的12个重组后的参数，即可完成动力学的建模。
在一种或多种实施方式中，在采集采样点时，需保证采样点尽可能分布在整个工作空间内，以提高辨识精度。在对KUKA LBR Med 7 R800的运动轨迹进行采样时，可采用对各个关节的工作空间进行均匀采样，获得随机关节位置。采样时刻应保证机械臂基本处于静止状态，采集当前时刻的关节位置和关节力矩信息，然后运动到下一位置。运动过程中可能会遇到奇异点位置，由于在奇异点位置，机械臂会缺失部分自由度，因此需要从数据集中去除奇异点附近的采样点。
将采集到的数据以如下方式堆叠到线性方程组里：
Figure PCTCN2020125842-appb-000019
上式中只有包含待辨识参数的矩阵u是未知的，因此可使用最小二乘法解算上式，将矩阵u解算出来，即辨识出了所有与动力学模型相关的参数。
在步骤(1)中，参数已经得到了辨识，步骤(2)将基于辨识出来的参数，完成末端执行器上外力的测量工作。
对于测量步骤，参数矩阵u为已知，矩阵Y与测量时的关节角有关，得到的矩阵τ int表示的是由机械臂本体重量提供给各关节的力矩，计算公式为：
Y(θ 1,θ 2,…θ n)·u＝τ int
将测量的关节力矩值与本体提供的力矩相减，即可得到外力导致的关节力矩值：
τ ext＝τ measure-τ int
外力在关节空间和笛卡尔空间之间有如下关系：
τ ext＝J·F ext
此处的F ext包含了笛卡尔坐标系下的力和力矩值，因此是六维的向量。通过对雅可比矩阵求伪逆，即可得到末端执行器上基于笛卡尔坐标系表示的外力F ext。采用关节力矩传感器的读数作为辨识程序的输入，较现有的多采用测电机电流估算关节力矩的方案，能有效提升辨识精度。
为了评估该方法和传统方法在静止或低速工况下的性能差距，设计了如下实验方法：将机械臂固定在一个非奇异的位置，使用推拉力计制造恒定拉力，并保持拉力方向始终垂直于法兰盘平面和法兰轴线。每个位置测量多组不同拉力、不同角度的数据，并且分别记录新方法和传统方法测量的力的大小，重复以上步骤，移动机械臂测量其他位置，尽量使采样位置遍布整个工作空间；力矩的评估与力的评估类似，只需要将拉力方向改为法兰盘切线方向和平行于法兰轴线方向即可制造不同方向的力矩，分别记录新方法和传统方法测量的力矩的大小。需要注意的是力矩的真实值计算需要用到法兰半径和拉力值的乘积，遵照以下公式：
τ＝d radius·F pull
针对在KUKA LBR Med 7 R800上的实验，该方法在87％采样数据上的准确性都较传统方法误差更小。综合误差率方面，该方法达到1.2％，相较于传统方法的2.0％有较大提升。
传统的方法适用范围更广，即使在高速和加速度变化较大的情况下也能较好的估计力的大小，但是缺点在于计算量较大，运算成本较高。与传统的机械臂参数辨识方法进行对比，本公开的方法在传统方法基础上做了改进。发明人对医疗机械臂工况的特殊性进行了分析。根绝分析得到的医疗机械臂工况的特殊性，对方法进行了简化。本公开将方法限定于静止或低速条件下的力反馈，大大减少了需要辨识的参数数量，降低了对计算能力的要求，并且使在此种工况下的力反馈更加精确。本公开的方法可用于多种机械臂辅助的外科手术，包括骨科手术、肿瘤切除手术、腹腔镜手术、胸腔镜手术等，还可应用于医疗检查，包括B超、放射检查。
最后应当说明的是，以上实施例仅用以说明本公开的技术方案，而非对本公开保护范围的限制，尽管参照较佳实施例对本公开作了详细地说明，本领域的普通技术人员应当理解，可以对本公开的技术方案进行修改或者等同替换，而不脱离本公开技术方案的实质和范围。
工业实用性
本公开对医疗机械臂工况的特殊性进行了分析。本公开的方法适用于静止或低速条件下的力反馈，在动力学方程中的关节速度和加速度在该种条件下接近于零，仅保留重力项以适应静止或低速的工况，得到动力学方程中重力项的矩阵。本公开的方法大大减少了需要辨识的参数数量，降低了对计算能力的要求，并且使在此种工况下的力反馈更加精确。

Claims (11)

  1. 一种对于静止或低速工况下机械臂高精度力反馈的方法，其中，包括以下步骤：
    S100，构建机械臂的各个关节坐标系，并计算关节的雅可比矩阵；
    S200，根据上述的关节坐标系，构建原点位于机械臂的每一段质心位置的坐标系，并计算质心位置的雅可比矩阵；
    S300，构建动力学方程中重力项的矩阵；
    S400，设置激励运动轨迹，在机械臂运动过程中采集得到关节位置和力矩数据；
    S500，将S400的数据代入到S300中，计算出结果；
    S600，将辨识的参数代入到动力学方程中，得到机械臂在特定位姿下由于本体产生的关节扭矩，将计算所得与实时测量的关节扭矩相减，得到施加外力导致的额外关节扭矩；
    S700，结合S100中得到的关节雅可比矩阵和S600中得到的额外关节扭矩，计算得到机械臂末端外力的大小和方向。
  2. 如权利要求1所述的方法，其中，所述S100步骤中，根据平移和欧拉角结合的方式构建机械臂各个关节坐标系。
  3. 如权利要求1或2所述的方法，其中，所述S300步骤中，进行参数分离，将与关节位置有关的参数放入一个矩阵，将与质量和质心有关的参数组合后放入另一个矩阵。
  4. 如权利要求1-3中任一项所述的方法，其中，所述S400步骤中，采样时保证机械臂处于静止状态，且采样点布满整个工作空间。
  5. 如权利要求3所述的方法，其中，所述与关节位置有关的参数包括关节角。
  6. 如权利要求3所述的方法，其中，所述与质量和质心有关的参数包括质量、质心位置和长度。
  7. 如权利要求1-6中任一项所述的方法，其中，所述S400步骤中，去除机械臂缺失部分或全部自由度的奇异点位置处的数据。
  8. 根据权利要求1-7中任一项所述的方法，其中，所述机械臂为多轴机械臂。
  9. 一种机械臂辅助的外科手术方法，包括使用机械臂对受试者进行手术，其中，使用根据权利要求1-8中任一项所述的对于静止或低速工况下机械臂高精度力反馈的方法进行机械臂的力反馈。
  10. 如权利要求9所述的外科手术方法，其中，所述机械臂为多轴机械臂。
  11. 一种具有处理器可执行的非易失的程序代码的计算机可读介质，其中，所述程 序代码使所述处理器执行所述权利要求1-8任一项所述方法。
PCT/CN2020/125842 2020-06-03 2020-11-02 一种对于静止或低速工况下机械臂高精度力反馈的方法、机械臂辅助的外科手术方法和具有处理器可执行的非易失的程序代码的计算机可读介质 WO2021243945A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202010494425.8A CN111618859B (zh) 2020-06-03 2020-06-03 一种对于静止或低速工况下机械臂高精度力反馈的方法
CN202010494425.8 2020-06-03

Publications (1)

Publication Number Publication Date
WO2021243945A1 true WO2021243945A1 (zh) 2021-12-09

Family

ID=72256535

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2020/125842 WO2021243945A1 (zh) 2020-06-03 2020-11-02 一种对于静止或低速工况下机械臂高精度力反馈的方法、机械臂辅助的外科手术方法和具有处理器可执行的非易失的程序代码的计算机可读介质

Country Status (2)

Country Link
CN (1) CN111618859B (zh)
WO (1) WO2021243945A1 (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114102603A (zh) * 2021-12-13 2022-03-01 南京佗道医疗科技有限公司 一种基于笛卡尔空间的零力拖动方法
CN115081147A (zh) * 2022-07-20 2022-09-20 上海龙慧医疗科技有限公司 用于扩展无偏置七轴机械臂末端手术工具瞄准范围的方法
CN117584137A (zh) * 2024-01-18 2024-02-23 浙江大学 面向随机工况的液压机械臂带载模型在线修正方法及系统

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111618859B (zh) * 2020-06-03 2021-04-13 杭州键嘉机器人有限公司 一种对于静止或低速工况下机械臂高精度力反馈的方法
CN113715025A (zh) * 2021-09-10 2021-11-30 南京猫眼智能科技有限公司 自动拉伸机械手臂控制方法
CN114536330B (zh) * 2022-02-16 2023-10-20 中国医学科学院北京协和医院 基于多个位姿标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536329B (zh) * 2022-02-16 2024-05-17 中国医学科学院北京协和医院 基于复合标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536331B (zh) * 2022-02-16 2023-10-20 中国医学科学院北京协和医院 基于关联标识确定可形变机械臂的外部受力的方法及机器人系统
CN114603565B (zh) * 2022-05-10 2022-08-02 季华实验室 欠驱动机械臂及其控制方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130173060A1 (en) * 2012-01-04 2013-07-04 Hyundai Motor Company Method of operating a wearable robot
CN106483964A (zh) * 2015-08-31 2017-03-08 中南大学 一种基于接触力观测器的机器人柔顺控制方法
CN107590340A (zh) * 2017-09-15 2018-01-16 浙江大学 一种机械臂外力估计方法及装置
CN109202889A (zh) * 2017-06-30 2019-01-15 沈阳新松机器人自动化股份有限公司 一种柔性多关节机器人电流力控制系统及方法
CN110716557A (zh) * 2019-11-18 2020-01-21 河北工业大学 基于先验动力学知识的机器人参数识别及接触力监测方法
CN111015649A (zh) * 2019-10-12 2020-04-17 深圳先进技术研究院 一种驱控一体化控制系统
CN111053613A (zh) * 2019-12-10 2020-04-24 联博智能科技有限公司 一种运动追踪方法、装置、机械臂及计算机可读存储介质
CN111618859A (zh) * 2020-06-03 2020-09-04 甘博涵 一种对于静止或低速工况下机械臂高精度力反馈的方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10065312B1 (en) * 2015-07-13 2018-09-04 The United States Of America, As Represented By The Secretary Of The Navy Unscented optimization and control allocation
KR101575487B1 (ko) * 2014-06-11 2015-12-08 현대자동차주식회사 로봇의 대상물 무게 및 무게중심 산출시스템 및 그 제어방법
CN107028663B (zh) * 2017-04-18 2019-04-12 中国科学院重庆绿色智能技术研究院 一种主从式手术机器人控制方法
CN107433590B (zh) * 2017-07-31 2020-08-18 上海宇航系统工程研究所 一种基于机械臂负载质量和传感器零漂在线辨识的重力补偿方法
CN109514602B (zh) * 2018-11-08 2020-06-16 东南大学 一种基于负载自适应辨识的工业机器人力矩补偿控制方法
CN111037568B (zh) * 2019-12-30 2023-04-07 上海新时达机器人有限公司 四轴机器人末端负载辨识方法及模块

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130173060A1 (en) * 2012-01-04 2013-07-04 Hyundai Motor Company Method of operating a wearable robot
CN106483964A (zh) * 2015-08-31 2017-03-08 中南大学 一种基于接触力观测器的机器人柔顺控制方法
CN109202889A (zh) * 2017-06-30 2019-01-15 沈阳新松机器人自动化股份有限公司 一种柔性多关节机器人电流力控制系统及方法
CN107590340A (zh) * 2017-09-15 2018-01-16 浙江大学 一种机械臂外力估计方法及装置
CN111015649A (zh) * 2019-10-12 2020-04-17 深圳先进技术研究院 一种驱控一体化控制系统
CN110716557A (zh) * 2019-11-18 2020-01-21 河北工业大学 基于先验动力学知识的机器人参数识别及接触力监测方法
CN111053613A (zh) * 2019-12-10 2020-04-24 联博智能科技有限公司 一种运动追踪方法、装置、机械臂及计算机可读存储介质
CN111618859A (zh) * 2020-06-03 2020-09-04 甘博涵 一种对于静止或低速工况下机械臂高精度力反馈的方法

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114102603A (zh) * 2021-12-13 2022-03-01 南京佗道医疗科技有限公司 一种基于笛卡尔空间的零力拖动方法
CN114102603B (zh) * 2021-12-13 2023-12-26 佗道医疗科技有限公司 一种基于笛卡尔空间的零力拖动方法
CN115081147A (zh) * 2022-07-20 2022-09-20 上海龙慧医疗科技有限公司 用于扩展无偏置七轴机械臂末端手术工具瞄准范围的方法
CN117584137A (zh) * 2024-01-18 2024-02-23 浙江大学 面向随机工况的液压机械臂带载模型在线修正方法及系统
CN117584137B (zh) * 2024-01-18 2024-03-26 浙江大学 面向随机工况的液压机械臂带载模型在线修正方法及系统

Also Published As

Publication number Publication date
CN111618859A (zh) 2020-09-04
CN111618859B (zh) 2021-04-13

Similar Documents

Publication Publication Date Title
WO2021243945A1 (zh) 一种对于静止或低速工况下机械臂高精度力反馈的方法、机械臂辅助的外科手术方法和具有处理器可执行的非易失的程序代码的计算机可读介质
CN107995885B (zh) 一种坐标系标定方法 、系统及装置
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
Bennett et al. Autonomous robot calibration for hand-eye coordination
CN107703748B (zh) 一种基于偏置板设计的重载机器人静态刚度辨识方法
CN110815206B (zh) 一种Stewart型并联机器人运动学标定方法
CN110666794B (zh) 一种机器人末端工具负载力和力矩补偿的方法
WO2018196232A1 (zh) 机器人和末端执行器的自动标定方法及系统
CN107038275B (zh) 一种机械臂误差分析方法
Huang et al. A 3D static modeling method and experimental verification of continuum robots based on pseudo-rigid body theory
Fortin-Côté et al. Improving cable driven parallel robot accuracy through angular position sensors
An et al. Geometrical kinematic solution of serial spatial manipulators using screw theory
Xiao et al. A new fixed axis-invariant based calibration approach to improve absolute positioning accuracy of manipulators
Wiese et al. Kinematic modeling of a soft pneumatic actuator using cubic hermite splines
CN109866224B (zh) 一种机器人雅可比矩阵计算方法、装置及存储介质
CN110900608B (zh) 基于最优测量构型选择的机器人运动学标定方法
KR20130066250A (ko) 인간형 관절 구성의 ６축 로봇 팔 제어 장치 및 방법
Figueroa et al. Joint origin identification of articulated robots with marker-based multi-camera optical tracking systems
Lippiello et al. Robot interaction control using force and vision
Zhang et al. Kinematic singularity analysis and simulation for 7dof anthropomorphic manipulator
CN115091455A (zh) 一种工业机器人定位误差补偿方法
Bouzgou et al. Singularity analysis and illustration of inverse kinematic solutions of 6 dof fanuc 200ic robot in virtual environment
Kmich et al. Image-Based Visual Servoing Techniques for Robot Control
Liu et al. Trajectory planning of robot manipulators based on unit quaternion
An et al. Model-based control of a direct drive arm. I. Building models

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20939273

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 20939273

Country of ref document: EP

Kind code of ref document: A1