CN112276959A - Geometric parameter and joint zero position self-calibration method and device, electronic equipment and medium - Google Patents

Geometric parameter and joint zero position self-calibration method and device, electronic equipment and medium Download PDF

Info

Publication number
CN112276959A
CN112276959A CN202011560013.6A CN202011560013A CN112276959A CN 112276959 A CN112276959 A CN 112276959A CN 202011560013 A CN202011560013 A CN 202011560013A CN 112276959 A CN112276959 A CN 112276959A
Authority
CN
China
Prior art keywords
joint
foot
robot
error
feet
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.)
Granted
Application number
CN202011560013.6A
Other languages
Chinese (zh)
Other versions
CN112276959B (en
Inventor
孔令雨
黄冠宇
高成志
谢也
谢安桓
张丹
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202011560013.6A priority Critical patent/CN112276959B/en
Publication of CN112276959A publication Critical patent/CN112276959A/en
Application granted granted Critical
Publication of CN112276959B publication Critical patent/CN112276959B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • 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
    • 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
    • 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

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

本发明公开了一种几何参数与关节零位自标定方法、装置、电子设备及介质，具体可以用于对足式机器人各个构件的尺寸参数以及驱动关节零位进行标定，提高机器人的定位与控制精度。所述方法通过使用一块尺寸参数明确的标定板，将机器人足部固定其上，通过调节机器人的关节位置，利用放置于机器人身体上的惯性测量单元以及关节读数，在建立机器人标定模型的基础上，即可实现对机器人结构尺寸参数与关节零位的标定。本发明具有低成本及操作简便等优势，适用于对具有不同构型、不同腿足数目的机器人进行参数标定。

Figure 202011560013

The invention discloses a method, device, electronic equipment and medium for self-calibration of geometric parameters and joint zero position, which can be specifically used for calibrating the size parameters of each component of a foot-type robot and the zero position of driving joints, so as to improve the positioning and control of the robot precision. The method uses a calibration plate with definite size parameters to fix the robot feet on it, adjusts the joint positions of the robot, uses the inertial measurement unit and joint readings placed on the robot body, and establishes a robot calibration model on the basis of , the calibration of the robot structure size parameters and the joint zero position can be realized. The invention has the advantages of low cost, simple operation and the like, and is suitable for parameter calibration of robots with different configurations and different numbers of legs and feet.

Figure 202011560013

Description

几何参数与关节零位自标定方法、装置、电子设备及介质Geometric parameter and joint zero self-calibration method, device, electronic equipment and medium

技术领域technical field

本发明涉机器人及参数辨识领域，尤其涉及一种几何参数与关节零位自标定方法、装置、电子设备及介质。The invention relates to the field of robot and parameter identification, in particular to a method, device, electronic device and medium for self-calibration of geometric parameters and joint zero position.

背景技术Background technique

在移动机器人中，足式机器人在地形适应能力方面具有显著优势，具备复杂地面行走、躲避障碍等能力，但存在控制难度高、运动稳定性差等问题。对足式机器人进行高精度控制是实现其稳定运动的关键问题。然而，受到机械加工、装配等方面误差的影响，足式机器人的几何参数、运动关节零位与其设计值之间都会存在偏差，在此基础上对机器人进行运动解算，必然会导致足部的位姿偏差。因此，有必要对足式机器人的几何参数与关节零位进行校准，提高足部的定位精度，为机器人的稳定控制奠定基础。Among mobile robots, footed robots have significant advantages in terrain adaptation, and have the ability to walk on complex ground and avoid obstacles, but have problems such as high control difficulty and poor motion stability. High-precision control of a footed robot is a key issue to achieve its stable motion. However, due to the influence of errors in machining, assembly, etc., there will be deviations between the geometric parameters of the footed robot, the zero position of the motion joints and their design values. On this basis, the motion calculation of the robot will inevitably lead to the foot. Pose deviation. Therefore, it is necessary to calibrate the geometric parameters of the footed robot and the joint zero position to improve the positioning accuracy of the foot and lay the foundation for the stable control of the robot.

经过对现有技术的检索发现，目前对足式机器人几何参数与关节零位的标定手段较少，且少数该方面的技术手段多针对机器人中的驱动关节进行标定，缺乏整体性考量，从而由构件尺寸参数造成的定位误差难以被控制系统补偿。比如，中国专利申请公布号CN110374961A，公开了一种足式机器人液压作动器自标定装置及标定方法，用于单独对机器人的驱动系统进行标定；中国专利申请公布号CN107065558A，公开了一种基于机身姿态角度校正的六足机器人关节角度标定方法，用于对六足机器人的关节角度偏差进行标定。After searching the existing technology, it is found that there are few calibration methods for the geometric parameters and joint zero position of the footed robot at present, and the few technical methods in this area are mainly for the calibration of the driving joints in the robot, which lacks overall consideration. The positioning error caused by the component size parameters is difficult to be compensated by the control system. For example, Chinese Patent Application Publication No. CN110374961A discloses a self-calibration device and a calibration method for a foot-type robot hydraulic actuator, which are used to independently calibrate the driving system of the robot; Chinese Patent Application Publication No. CN107065558A discloses a A hexapod robot joint angle calibration method for body attitude angle correction is used to calibrate the joint angle deviation of the hexapod robot.

发明内容SUMMARY OF THE INVENTION

本发明实施例的目的是提供一种几何参数与关节零位自标定方法、装置、电子设备及介质，以至少解决相关技术中标定参数考虑不完善、标定方法通用性不强的问题。The purpose of the embodiments of the present invention is to provide a method, device, electronic device and medium for self-calibration of geometric parameters and joint zero position, so as to at least solve the problems of imperfect consideration of calibration parameters and weak generality of calibration methods in the related art.

为了达到上述目的，本发明实施例所采用的技术方案如下：In order to achieve the above purpose, the technical solutions adopted in the embodiments of the present invention are as follows:

根据本发明实施例的第一方面，提供一种几何参数与关节零位自标定方法，该方法用于足式机器人的几何参数与关节零位自标定，该方法包括：获取所述足式机器人身体的姿态以及关节位置数据，其中所述足式机器人足部被固定在标定板上；根据所述姿态以及关节位置数据，分别构建足式机器人身体与足部之间的运动学正解模型、足式机器人身体到足部之间的第一位姿误差模型以及足式机器人各足部之间的第二位姿误差模型，其中所述运动学正解模型用于根据身体的姿态以及关节位置数据求解对应的足部位姿，所述第一位姿误差模型用于描述所对应的腿部连杆尺寸误差以及运动关节零位误差到足部位姿误差的传递关系；所述第二位姿误差模型用于描述不同足部之间的受对应腿部几何参数偏差以及关节零位偏差影响的位姿误差关系；基于所述的运动学正解模型、第一位姿误差模型和第二位姿误差模型，利用多组位形下足式机器人的姿态以及关节位置数据，对足式机器人几何参数与关节零位进行辨识计算，直至残差收敛至给定阈值或迭代计算已收敛，最终得到的关节零位及几何参数。According to a first aspect of the embodiments of the present invention, there is provided a method for self-calibration of geometric parameters and joint zero positions. The method is used for self-calibration of geometric parameters and joint zero positions of a footed robot. The method includes: obtaining the footed robot. Body posture and joint position data, wherein the foot of the footed robot is fixed on the calibration plate; according to the posture and joint position data, respectively construct a positive kinematics model of the footed robot between the body and the foot. The first pose error model between the body and the foot of the foot robot and the second pose error model between the feet of the foot robot, wherein the kinematics positive solution model is used to solve the problem according to the body posture and joint position data Corresponding foot position, the first position error model is used to describe the corresponding leg link size error and the transfer relationship between the motion joint zero position error and the foot position error; the second position error model is used. It is used to describe the pose error relationship between different feet affected by the geometric parameter deviation of the corresponding leg and the joint zero deviation; based on the positive kinematic solution model, the first pose error model and the second pose error model, Using the posture and joint position data of the footed robot under multiple sets of configurations, the geometric parameters and joint zero position of the footed robot are identified and calculated until the residual converges to a given threshold or the iterative calculation has converged, and the joint zero position is finally obtained. and geometric parameters.

根据本发明实施例的第二方面，提供一种几何参数与关节零位自标定装置，该装置用于足式机器人，包括：获取单元，用于所述获取足式机器人身体的姿态以及关节位置数据，其中所述足式机器人被安装在标定板上；模型构建单元，用于根据所述姿态以及关节位置数据，分别构建足式机器人身体与足部之间的运动学正解模型、足式机器人身体到足部之间的第一位姿误差模型以及足式机器人各足部之间的第二位姿误差模型，其中所述运动学正解模型用于根据身体的姿态以及关节位置数据求解对应的足部位姿，所述第一位姿误差模型用于描述所对应的腿部连杆尺寸误差以及运动关节零位误差到足部位姿误差的传递关系；所述第二位姿误差模型用于描述不同足部之间的受对应腿部几何参数偏差以及关节零位偏差影响的位姿误差关系；计算单元，用于基于所述的运动学正解模型、第一位姿误差模型和第二位姿误差模型，利用多组位形下足式机器人的姿态以及关节位置数据，对足式机器人几何参数与关节零位进行辨识计算，直至残差收敛至给定阈值，最终得到的关节零位及几何参数。According to a second aspect of the embodiments of the present invention, there is provided a geometric parameter and joint zero position self-calibration device. The device is used for a footed robot and includes: an acquisition unit for acquiring the posture and joint positions of the footed robot body data, wherein the footed robot is installed on the calibration plate; the model building unit is used to construct the positive kinematics model between the body and the foot of the footed robot, the footed robot, according to the posture and joint position data, respectively. The first pose error model between the body and the foot and the second pose error model between the feet of the footed robot, wherein the kinematics positive solution model is used to solve the corresponding Foot position, the first position error model is used to describe the corresponding leg link size error and the transfer relationship from the motion joint zero position error to the foot position error; the second position error model is used to describe The pose error relationship between different feet affected by the geometric parameter deviation of the corresponding leg and the joint zero position deviation; the calculation unit is used for the positive kinematic solution model, the first pose error model and the second pose based on the described The error model uses the posture and joint position data of the footed robot under multiple configurations to identify and calculate the geometric parameters and joint zero position of the footed robot until the residual converges to a given threshold, and the joint zero position and geometry are finally obtained. parameter.

根据本发明实施例的第三方面，提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的方法。According to a third aspect of the embodiments of the present invention, there is provided an electronic device, comprising: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more programs Execution of the one or more processors causes the one or more processors to implement the method as described in the first aspect.

根据本发明实施例的第四方面，提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如第一方面所述的方法。According to a fourth aspect of the embodiments of the present invention, there is provided a computer-readable storage medium on which a computer program is stored, and when the program is executed by a processor, implements the method according to the first aspect.

根据以上技术方案，本发明实施例的有益效果如下：在仅依靠一块标定板的情况下，所建立的几何与关节零位误差与位姿误差的传递关系模型，可以同时考虑机器人构件的几何尺寸误差与驱动关节零位误差对机器人定位精度的影响，在通过获取多个位形下机器人的身体姿态及关节位置信息的情况下，即可同步完成机器人构件的几何参数及驱动关节零位自标定，通过将标定后的结果补偿到控制系统中，即可提高机器人足部的定位精度，提高机器人运动控制效果。According to the above technical solutions, the beneficial effects of the embodiments of the present invention are as follows: in the case of only relying on one calibration plate, the established transfer relationship model between geometry and joint zero position error and pose error can simultaneously consider the geometric dimensions of the robot components The influence of the error and the zero position error of the driving joint on the positioning accuracy of the robot. By obtaining the body posture and joint position information of the robot in multiple configurations, the geometric parameters of the robot components and the self-calibration of the driving joint zero position can be synchronously completed. , by compensating the calibrated result into the control system, the positioning accuracy of the robot foot can be improved, and the motion control effect of the robot can be improved.

附图说明Description of drawings

此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：The accompanying drawings described herein are used to provide further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the attached image:

图1是根据一示例性实施例示出的一种几何参数与关节零位自标定方法的流程图。FIG. 1 is a flow chart of a method for self-calibration of geometric parameters and joint zero position according to an exemplary embodiment.

图2是根据一示例性实施例示出的双足机器人结构示意图。FIG. 2 is a schematic structural diagram of a biped robot according to an exemplary embodiment.

图3是根据一示例性实施例示出的标定板示意图。FIG. 3 is a schematic diagram of a calibration plate according to an exemplary embodiment.

图4是根据一示例性实施例示出的双足机器人结构示意图。FIG. 4 is a schematic structural diagram of a biped robot according to an exemplary embodiment.

图5是根据一示例性实施例示出的几何参数与关节零位的辨识计算流程图。FIG. 5 is a flow chart of identification and calculation of geometric parameters and joint zero positions according to an exemplary embodiment.

图6是根据一示例性实施例示出的一种几何参数与关节零位自标定装置的框图。Fig. 6 is a block diagram of a geometric parameter and joint zero position self-calibration device according to an exemplary embodiment.

具体实施方式Detailed ways

下面根据附图和优选实施例详细描述本发明，本发明的目的和效果将变得更加明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。The present invention will be described in detail below according to the accompanying drawings and preferred embodiments, and the purpose and effects of the present invention will become clearer. It should be understood that the specific embodiments described herein are only used to explain the present invention, but not to limit the present invention.

实施例1：Example 1:

图1是根据一示例性实施例示出的一种几何参数与关节零位自标定方法的流程图，参考图1，本实施例提供一种几何参数与关节零位自标定方法，该方法用于足式机器人的几何参数与关节零位自标定，该方法包括以下步骤：FIG. 1 is a flow chart of a method for self-calibration of geometric parameters and joint zero position according to an exemplary embodiment. Referring to FIG. 1 , this embodiment provides a method for self-calibration of geometric parameters and joint zero position. The method is used for The self-calibration of geometric parameters and joint zero position of the footed robot includes the following steps:

步骤S101，获取足式机器人身体的姿态以及关节位置数据，其中所述足式机器人被安装在标定板2上；Step S101, acquiring the posture and joint position data of the footed robot body, wherein the footed robot is installed on the calibration plate 2;

具体地，图2是根据一示例性实施例示出的双足机器人结构示意图，图3是根据一示例 性实施例示出的标定板示意图，图4是根据一示例性实施例示出的双足机器人结构示意图。 所述的标定板上加工有与所述足式机器人足部数量相同的定位部，用于连接足式机器人足 部，且标定出足部的位置关系。需要说明的是，所述的足式机器人形式包括但不限于双足、 四足、六足等构型，足部与身体之间由运动关节与连杆相继连接而成；以某双足机器人为 例，如图1-3所示，其身体7分别与左足部14和右足部16之间依次通过第一运动关节8、第二 运动关节9、第三运动关节10、第一连杆11、第四运动关节12、第二连杆13和第五运动关节15 连接，其中，第一运动关节8、第二运动关节9和第三运动关节10整体构成足式机器人的髋关 节，第四运动关节12为膝关节，第五运动关节15为踝关节；标定板上具有高精度加工而成的 且与机器人足部数量相同的用于连接机器人足部的第一定位部3和第二定位部5，并分别在 其上建立左足部坐标系4与右足部坐标系6，第一定位部3和第二定位部5之间具有精确的位 置关系，并通过包含但不限于螺栓的连接方式与机器人左足部14和右足部16刚性连接。并 定义坐标系

Figure 260876DEST_PATH_IMAGE001
相对于
Figure 646858DEST_PATH_IMAGE002
具有精确的位姿关系
Figure 144704DEST_PATH_IMAGE003
。 Specifically, FIG. 2 is a schematic diagram of the structure of a biped robot according to an exemplary embodiment, FIG. 3 is a schematic diagram of a calibration board according to an exemplary embodiment, and FIG. 4 is a schematic diagram of the structure of a biped robot according to an exemplary embodiment. Schematic. The calibration plate is provided with the same number of positioning parts as the feet of the foot-type robot, which are used for connecting the feet of the foot-type robot and calibrating the positional relationship of the feet. It should be noted that the form of the footed robot includes but is not limited to configurations such as bipedal, quadrupedal, hexapodal, etc. The foot and the body are successively connected by kinematic joints and connecting rods; For example, as shown in FIGS. 1-3 , the body 7 passes through the first motion joint 8 , the second motion joint 9 , the third motion joint 10 , and the first link 11 in sequence between the left foot 14 and the right foot 16 respectively. , the fourth kinematic joint 12, the second link 13 and the fifth kinematic joint 15 are connected, wherein the first kinematic joint 8, the second kinematic joint 9 and the third kinematic joint 10 integrally constitute the hip joint of the footed robot, and the fourth kinematic joint 8 The kinematic joint 12 is the knee joint, and the fifth kinematic joint 15 is the ankle joint; the calibration plate has the first positioning part 3 and the second positioning part 3 and the second positioning part for connecting the robot feet which are processed with high precision and have the same number as the robot feet. part 5, and respectively establish the left foot coordinate system 4 and the right foot coordinate system 6 thereon, the first positioning part 3 and the second positioning part 5 have a precise positional relationship, and are connected by means including but not limited to bolts Rigidly connected to the left foot 14 and right foot 16 of the robot. and define the coordinate system
Figure 260876DEST_PATH_IMAGE001
relative to
Figure 646858DEST_PATH_IMAGE002
with precise pose relationship
Figure 144704DEST_PATH_IMAGE003
.

在获取足式机器人身体的姿态以及关节位置数据前，需要将所述足式机器人足部安装在所述的标定板上，其中获取足式机器人身体的姿态数据的步骤，包括：Before acquiring the posture and joint position data of the footed robot body, the foot of the footed robot needs to be installed on the calibration plate, and the steps of acquiring the posture data of the footed robot body include:

通过传感器读取足式机器人身体姿态信息，该传感器包括但不限于惯性测量单元；定 义由传感器读出的身体姿态相对于全局坐标系的坐标

Figure 8755DEST_PATH_IMAGE004
为： Read the body posture information of the footed robot through sensors, including but not limited to inertial measurement units; define the coordinates of the body posture read by the sensor relative to the global coordinate system
Figure 8755DEST_PATH_IMAGE004
for:

Figure 291969DEST_PATH_IMAGE005
Figure 291969DEST_PATH_IMAGE005

其中，

Figure 380010DEST_PATH_IMAGE006
分别表示足式机器人身体坐标系相对于全局坐标系的转动RPY欧 拉角，则身体坐标系相对于全局坐标系的姿态矩阵
Figure 115885DEST_PATH_IMAGE007
表示为： in,
Figure 380010DEST_PATH_IMAGE006
respectively represent the rotation RPY Euler angle of the body coordinate system of the footed robot relative to the global coordinate system, then the posture matrix of the body coordinate system relative to the global coordinate system
Figure 115885DEST_PATH_IMAGE007
Expressed as:

Figure 38754DEST_PATH_IMAGE008
Figure 38754DEST_PATH_IMAGE008

其中，

Figure 176474DEST_PATH_IMAGE009
表示沿x轴转动
Figure 638680DEST_PATH_IMAGE010
角度的姿态矩阵，
Figure 861851DEST_PATH_IMAGE011
表示沿z轴转动
Figure 51392DEST_PATH_IMAGE012
角度的 姿态矩阵，
Figure 43619DEST_PATH_IMAGE013
表示沿y轴转动
Figure 942305DEST_PATH_IMAGE014
角度的姿态矩阵；
Figure 652772DEST_PATH_IMAGE015
，
Figure 646005DEST_PATH_IMAGE016
，
Figure 555055DEST_PATH_IMAGE017
，
Figure 359063DEST_PATH_IMAGE018
，
Figure 556826DEST_PATH_IMAGE019
，
Figure 104482DEST_PATH_IMAGE020
分别 表示
Figure 320569DEST_PATH_IMAGE021
，
Figure 561057DEST_PATH_IMAGE022
的缩写； in,
Figure 176474DEST_PATH_IMAGE009
Represents a rotation along the x- axis
Figure 638680DEST_PATH_IMAGE010
The attitude matrix of angles,
Figure 861851DEST_PATH_IMAGE011
Represents a rotation along the z- axis
Figure 51392DEST_PATH_IMAGE012
The attitude matrix of angles,
Figure 43619DEST_PATH_IMAGE013
Represents a rotation along the y- axis
Figure 942305DEST_PATH_IMAGE014
The attitude matrix of the angle;
Figure 652772DEST_PATH_IMAGE015
,
Figure 646005DEST_PATH_IMAGE016
,
Figure 555055DEST_PATH_IMAGE017
,
Figure 359063DEST_PATH_IMAGE018
,
Figure 556826DEST_PATH_IMAGE019
,
Figure 104482DEST_PATH_IMAGE020
Respectively
Figure 320569DEST_PATH_IMAGE021
,
Figure 561057DEST_PATH_IMAGE022
abbreviation of;

定义机器人身体坐标系相对于全局坐标系的位置矢量

Figure 980537DEST_PATH_IMAGE023
为： Defines the position vector of the robot body coordinate system relative to the global coordinate system
Figure 980537DEST_PATH_IMAGE023
for:

Figure 66305DEST_PATH_IMAGE024
Figure 66305DEST_PATH_IMAGE024

其中，x、y、z分别表示机器人身体坐标系相对于全局坐标系x、y、z轴方向的位置坐标。Wherein, x, y, and z represent the position coordinates of the robot body coordinate system relative to the x, y, and z axis directions of the global coordinate system, respectively.

获取足式机器人身体的关节位置数据的步骤，包括：The steps of obtaining joint position data of the footed robot body include:

通过安装于运动关节上的传感器读取足式机器人第i条腿的运动关节位置信息，该传 感器包括但不限于编码器、光栅尺等；并将每条腿中的运动关节位置信息

Figure 136898DEST_PATH_IMAGE025
合并并写成向 量的形式： Read the kinematic joint position information of the i -th leg of the footed robot through the sensor installed on the kinematic joint, the sensor includes but not limited to encoder, grating ruler, etc.; and combine the kinematic joint position information in each leg
Figure 136898DEST_PATH_IMAGE025
Combine and write as a vector:

Figure 548288DEST_PATH_IMAGE026
Figure 548288DEST_PATH_IMAGE026

其中

Figure 455064DEST_PATH_IMAGE027
表示第i条腿第n个运动关节位置信息。 in
Figure 455064DEST_PATH_IMAGE027
Indicates the position information of the nth kinematic joint of the ith leg.

步骤S102，根据所述姿态以及关节位置数据，分别构建足式机器人身体与足部之间的运动学正解模型、足式机器人身体到足部之间的第一位姿误差模型以及足式机器人各足部之间的第二位姿误差模型，其中所述运动学正解模型用于根据身体的姿态以及关节位置数据求解对应的足部位姿，所述第一位姿误差模型用于描述所对应的腿部连杆尺寸误差以及运动关节零位误差到足部位姿误差的传递关系；所述第二位姿误差模型用于描述不同足部之间的受对应腿部几何参数偏差以及关节零位偏差影响的位姿误差关系；Step S102, according to the posture and joint position data, respectively construct the positive kinematics model between the body and the foot of the footed robot, the first posture error model between the body and the foot of the footed robot, and each of the footed robots. The second pose error model between the feet, wherein the kinematics positive solution model is used to solve the corresponding foot pose according to the body pose and joint position data, and the first pose error model is used to describe the corresponding The transfer relationship between the leg link size error and the motion joint zero position error to the foot position error; the second position error model is used to describe the corresponding leg geometric parameter deviation and joint zero position deviation between different feet Affected pose error relationship;

具体地，构建足式机器人身体与足部之间的运动学正解模型的步骤，所述的运动学正解模型的建模方法包括但不限于闭环矢量法、D-H参数法以及指数积方法，本实例采用以下方法为例，包括：Specifically, the steps of constructing a positive kinematics model between the body and the foot of the footed robot, the modeling methods of the positive kinematics model include but are not limited to closed-loop vector method, D-H parameter method and exponential product method, this example Take the following methods as examples, including:

对于每条腿，根据机器人腿部构件的几何尺寸参数以及运动关节的位置信息，推导出对应的足部相对于机器人身体坐标系的位置及姿态信息。For each leg, the position and posture information of the corresponding foot relative to the robot body coordinate system are derived according to the geometric dimension parameters of the robot leg components and the position information of the kinematic joints.

对于第i条腿，足式机器人身体与足部之间的运动学正解模型表示为：For the i -th leg, the positive kinematics model between the body and the foot of the footed robot is expressed as:

Figure 141260DEST_PATH_IMAGE028
Figure 141260DEST_PATH_IMAGE028

其中，

Figure 82671DEST_PATH_IMAGE029
表示足式机器人身体与足部之间的运动学正解函数；
Figure 914230DEST_PATH_IMAGE030
表示第i个 足部相对于全局坐标系的位姿矢量，由位置与姿态矢量构成，即
Figure 42723DEST_PATH_IMAGE031
，其中
Figure 1451DEST_PATH_IMAGE032
与
Figure 266211DEST_PATH_IMAGE033
分别表示第i个足部相对于全局 坐标系的位置及姿态矢量，
Figure 268671DEST_PATH_IMAGE034
表示位置分量，
Figure 150039DEST_PATH_IMAGE035
表 示描述姿态的欧拉角；
Figure 443617DEST_PATH_IMAGE036
表示第i条腿中各构件的几何参数构成的矢量；
Figure 94041DEST_PATH_IMAGE037
表 示足式机器人身体坐标系相对于全局坐标系的位姿矢量。 in,
Figure 82671DEST_PATH_IMAGE029
Represents the positive kinematic solution function between the body and the foot of the footed robot;
Figure 914230DEST_PATH_IMAGE030
Represents the pose vector of the i -th foot relative to the global coordinate system, which is composed of the position and the pose vector, namely
Figure 42723DEST_PATH_IMAGE031
,in
Figure 1451DEST_PATH_IMAGE032
and
Figure 266211DEST_PATH_IMAGE033
respectively represent the position and attitude vector of the i -th foot relative to the global coordinate system,
Figure 268671DEST_PATH_IMAGE034
represents the position component,
Figure 150039DEST_PATH_IMAGE035
represents the Euler angles describing the pose;
Figure 443617DEST_PATH_IMAGE036
The vector representing the geometric parameters of each member in the i -th leg;
Figure 94041DEST_PATH_IMAGE037
Represents the pose vector of the footed robot body coordinate system relative to the global coordinate system.

构建足式机器人身体到足部之间的第一位姿误差模型的步骤，包括：The steps for constructing the first pose error model between the body and the foot of a footed robot include:

对于每条腿，根据机器人腿部构件的几何尺寸参数以及运动关节的位置信息，构建出腿部构件的几何尺寸误差与运动关节零位偏差到对应的足部的姿态偏差的传递关系，该传递关系为第一位姿误差模型。For each leg, according to the geometric parameters of the robot leg components and the position information of the kinematic joints, the transfer relationship between the geometric size error of the leg components and the zero position deviation of the kinematic joints to the attitude deviation of the corresponding foot is constructed. The relationship is the first pose error model.

具体地，足式机器人身体到足部之间的姿态误差模型表示为：Specifically, the attitude error model between the body and the foot of the foot robot is expressed as:

Figure 752556DEST_PATH_IMAGE038
Figure 752556DEST_PATH_IMAGE038

其中，

Figure 370488DEST_PATH_IMAGE039
表示足部理论与实际位姿之间的偏差，
Figure 139861DEST_PATH_IMAGE040
表示
Figure 910371DEST_PATH_IMAGE041
对应的误差参数，
Figure 739786DEST_PATH_IMAGE042
为 第i条腿中关节的零位偏差，
Figure 862593DEST_PATH_IMAGE043
分别表示
Figure 701236DEST_PATH_IMAGE044
与足部位姿偏差的传递关系矩阵； in,
Figure 370488DEST_PATH_IMAGE039
represents the deviation between the theoretical and actual pose of the foot,
Figure 139861DEST_PATH_IMAGE040
express
Figure 910371DEST_PATH_IMAGE041
The corresponding error parameters,
Figure 739786DEST_PATH_IMAGE042
is the zero deviation of the joint in the i -th leg,
Figure 862593DEST_PATH_IMAGE043
Respectively
Figure 701236DEST_PATH_IMAGE044
The transfer relationship matrix with the posture deviation of the foot;

除去与位置误差有关的参数，最终用于标定的第一位姿误差模型表示为：After removing the parameters related to the position error, the final first pose error model used for calibration is expressed as:

Figure 60673DEST_PATH_IMAGE045
Figure 60673DEST_PATH_IMAGE045

其中，

Figure 123307DEST_PATH_IMAGE046
表示标定板上足部定位装置坐标系在全局坐标系下的姿态矢量，
Figure 200984DEST_PATH_IMAGE047
为通过 运动学计算得到的足部相对于全局坐标系的姿态矢量，
Figure 92586DEST_PATH_IMAGE048
分别表示3维单位矩阵及三 维零矩阵。 in,
Figure 123307DEST_PATH_IMAGE046
represents the attitude vector of the coordinate system of the foot positioning device on the calibration board in the global coordinate system,
Figure 200984DEST_PATH_IMAGE047
is the pose vector of the foot relative to the global coordinate system calculated by kinematics,
Figure 92586DEST_PATH_IMAGE048
represent the 3-dimensional unit matrix and the three-dimensional zero matrix, respectively.

构建足式机器人各足部之间的第二位姿误差模型的步骤，包括：The steps of constructing the second pose error model between the feet of the footed robot include:

通过标定板可以获得预定的两个足部的真实位姿关系，同时利用这两个足部所对应腿部的运动学正解模型获得对应足部的理论位姿关系，以这两个足部的真实位姿与理论位姿的差值最小为优化目标，构建对应腿部几何参数误差与关节零位误差到足部位姿误差的传递关系，该传递关系为第二位姿误差模型。The real pose relationship of the two predetermined feet can be obtained through the calibration board, and the theoretical pose relationship of the corresponding feet can be obtained by using the kinematic positive solution model of the legs corresponding to the two feet. The minimum difference between the real pose and the theoretical pose is the optimization objective, and the transfer relationship between the corresponding leg geometric parameter error and the joint zero error to the foot pose error is constructed, and the transfer relationship is the second pose error model.

具体地，足部之间的位姿误差模型表示为：Specifically, the pose error model between the feet is expressed as:

Figure 306529DEST_PATH_IMAGE049
Figure 306529DEST_PATH_IMAGE049

其中，

Figure 743327DEST_PATH_IMAGE050
表示身体相对于世界坐标系的齐次变换矩阵，
Figure 308300DEST_PATH_IMAGE051
表示第i个足部位姿相对于 身体坐标系的齐次变换矩阵， in,
Figure 743327DEST_PATH_IMAGE050
represents the homogeneous transformation matrix of the body relative to the world coordinate system,
Figure 308300DEST_PATH_IMAGE051
represents the homogeneous transformation matrix of the i -th foot pose relative to the body coordinate system,

Figure 738014DEST_PATH_IMAGE052
Figure 738014DEST_PATH_IMAGE052

Figure 868781DEST_PATH_IMAGE053
分别表示第i个足部相对于身体坐标系的姿态矩阵与位置矢量；
Figure 476479DEST_PATH_IMAGE054
表示第j个 足部相对于第i个足部的位姿关系中实际值与理论值之间的偏差，
Figure 528749DEST_PATH_IMAGE055
表示标定板中足j相 对于足i的位姿关系矢量，为标定板的设计值，
Figure 247306DEST_PATH_IMAGE056
表示通过运动学正解计算得到足j相对于 足i的位姿关系矢量，
Figure 685110DEST_PATH_IMAGE057
对应的齐次变换矩阵可通过
Figure 463710DEST_PATH_IMAGE058
计算得出；
Figure 868781DEST_PATH_IMAGE053
respectively represent the posture matrix and position vector of the i -th foot relative to the body coordinate system;
Figure 476479DEST_PATH_IMAGE054
represents the deviation between the actual value and the theoretical value in the pose relationship of the jth foot relative to the ith foot,
Figure 528749DEST_PATH_IMAGE055
represents the pose relationship vector of foot j relative to foot i in the calibration board, which is the design value of the calibration board,
Figure 247306DEST_PATH_IMAGE056
Indicates that the pose relationship vector of foot j relative to foot i is obtained by calculating the positive kinematics solution,
Figure 685110DEST_PATH_IMAGE057
The corresponding homogeneous transformation matrix can be obtained by
Figure 463710DEST_PATH_IMAGE058
Calculated;

Figure 3276DEST_PATH_IMAGE059
Figure 3276DEST_PATH_IMAGE059

第二位姿误差模型中，函数

Figure 322262DEST_PATH_IMAGE060
表示齐次变换矩阵
Figure 630883DEST_PATH_IMAGE061
的伴随表征， In the second pose error model, the function
Figure 322262DEST_PATH_IMAGE060
represents a homogeneous transformation matrix
Figure 630883DEST_PATH_IMAGE061
the accompanying representation of ,

Figure 829652DEST_PATH_IMAGE062
Figure 829652DEST_PATH_IMAGE062

其中，

Figure 590935DEST_PATH_IMAGE063
为位置向量
Figure 916874DEST_PATH_IMAGE064
的反对称矩阵表征，写作： in,
Figure 590935DEST_PATH_IMAGE063
is the position vector
Figure 916874DEST_PATH_IMAGE064
The antisymmetric matrix representation of , written as:

Figure 814423DEST_PATH_IMAGE065
Figure 814423DEST_PATH_IMAGE065

x、y、z分别表示三维矢量

Figure 184093DEST_PATH_IMAGE066
中的元素。第二位姿误差模型中的
Figure 698251DEST_PATH_IMAGE067
分别表示
Figure 562302DEST_PATH_IMAGE068
与足部位姿偏差的传递关系矩阵，
Figure 579936DEST_PATH_IMAGE069
表示
Figure 386087DEST_PATH_IMAGE070
对应的误差参数，
Figure 918700DEST_PATH_IMAGE071
为
Figure 586442DEST_PATH_IMAGE072
对 应的零位偏差。 x, y, z represent three-dimensional vectors, respectively
Figure 184093DEST_PATH_IMAGE066
elements in . in the second pose error model
Figure 698251DEST_PATH_IMAGE067
Respectively
Figure 562302DEST_PATH_IMAGE068
The transfer relationship matrix with the foot position deviation,
Figure 579936DEST_PATH_IMAGE069
express
Figure 386087DEST_PATH_IMAGE070
The corresponding error parameters,
Figure 918700DEST_PATH_IMAGE071
for
Figure 586442DEST_PATH_IMAGE072
Corresponding zero offset.

此外，为保证姿态误差模型中误差传递矩阵的线性独立性，一般i、j选作相邻的腿 足，即假设机器人为n足，则存在n-1个足间位姿误差模型；与足部误差模型不同，由于标定 板各足部之间具有高精度的位姿关系，并且

Figure 458583DEST_PATH_IMAGE073
的误差模型中描述的是两个足部之间的位 姿关系，实际上不包含足式机器人身体的位置及姿态信息，因此
Figure 186367DEST_PATH_IMAGE073
的六维位姿误差参数 全部可得。将足式机器人中所有腿部对应的第一姿态误差模型和第二姿态误差模型写作矩 阵形式，即可得到用于进行足式机器人几何参数与关节零位自标定的误差模型，最终可以 写作下述形式为： In addition, in order to ensure the linear independence of the error transfer matrix in the attitude error model, i and j are generally selected as adjacent legs, that is, if the robot is n feet, there are n -1 pose error models between the feet; different error models, because the feet of the calibration board have a high-precision pose relationship, and
Figure 458583DEST_PATH_IMAGE073
The error model describes the pose relationship between the two feet, and does not actually contain the position and attitude information of the footed robot body, so
Figure 186367DEST_PATH_IMAGE073
The six-dimensional pose error parameters of all are available. The first attitude error model and the second attitude error model corresponding to all legs in the footed robot are written in matrix form, and the error model for the self-calibration of the geometric parameters of the footed robot and the joint zero position can be obtained, and finally it can be written as the following The form is:

Figure 658806DEST_PATH_IMAGE074
Figure 658806DEST_PATH_IMAGE074

其中，

Figure 599080DEST_PATH_IMAGE075
由
Figure 591307DEST_PATH_IMAGE076
与
Figure 489992DEST_PATH_IMAGE077
构成，
Figure 731618DEST_PATH_IMAGE078
与
Figure 996289DEST_PATH_IMAGE079
为对应于
Figure 108602DEST_PATH_IMAGE080
与
Figure 912610DEST_PATH_IMAGE081
的组合。 in,
Figure 599080DEST_PATH_IMAGE075
Depend on
Figure 591307DEST_PATH_IMAGE076
and
Figure 489992DEST_PATH_IMAGE077
constitute,
Figure 731618DEST_PATH_IMAGE078
and
Figure 996289DEST_PATH_IMAGE079
to correspond to
Figure 108602DEST_PATH_IMAGE080
and
Figure 912610DEST_PATH_IMAGE081
The combination.

以下以双足机器人为例，来对该步骤S103做进一步的详细说明。The step S103 is further described in detail below by taking a biped robot as an example.

在该实施例中，足式机器人的运动学正解模型采用D-H参数法构建。考虑到位姿误差模型构建时传统D-H参数在处理相邻关节轴线平行时会出现奇异的情况，因此采用改进的D-H方法（相邻关节轴线非平行时使用）与Hayati模型（相邻关节轴线平行时使用）分别构建，最终，本实施例中的运动学正解模型共包括3类位姿传递模型用以描述相邻构件的状态，即：In this embodiment, the positive kinematic solution model of the footed robot is constructed using the D-H parameter method. Considering the singularity of the traditional D-H parameters when the axes of adjacent joints are parallel when the pose error model is constructed, the improved D-H method (used when the axes of adjacent joints are not parallel) and the Hayati model (when the axes of adjacent joints are parallel) are adopted. Use ) to construct separately, and finally, the kinematics positive solution model in this embodiment includes a total of three types of pose transfer models to describe the states of adjacent components, namely:

Figure 844794DEST_PATH_IMAGE082
Figure 844794DEST_PATH_IMAGE082

其中，

Figure 641717DEST_PATH_IMAGE083
与
Figure 608536DEST_PATH_IMAGE084
分别表示移动与转动变换，括号中的第一个变量表示运动的 参考轴线，第二个变量表示具体运动量；
Figure 583445DEST_PATH_IMAGE085
表示机身坐标系到第一个运动关节8的位姿传 递关系，a、b、c表示沿对应轴线的平移量，
Figure 268505DEST_PATH_IMAGE086
表示沿对应轴线的转动量，
Figure 416589DEST_PATH_IMAGE087
表示 运动关节8的运动量；
Figure 487182DEST_PATH_IMAGE088
表示改进的D-H方法描述的坐标系间姿态变换关系，
Figure 898572DEST_PATH_IMAGE089
、 a、
Figure 805348DEST_PATH_IMAGE090
、
Figure 694807DEST_PATH_IMAGE091
表 示对应的D-H参数；
Figure 885486DEST_PATH_IMAGE092
表示Hayati模型描述的坐标系间姿态变换关系，
Figure 202197DEST_PATH_IMAGE093
表 示对应的D-H参数；根据实施例中足式机器人相邻关节的几何关系，可以得到足式机器人的 正向运动学模型为： in,
Figure 641717DEST_PATH_IMAGE083
and
Figure 608536DEST_PATH_IMAGE084
respectively represent the transformation of movement and rotation, the first variable in parentheses represents the reference axis of the movement, and the second variable represents the specific amount of movement;
Figure 583445DEST_PATH_IMAGE085
Represents the pose transfer relationship from the fuselage coordinate system to the first kinematic joint 8, a, b, c represent the translation along the corresponding axis,
Figure 268505DEST_PATH_IMAGE086
represents the amount of rotation along the corresponding axis,
Figure 416589DEST_PATH_IMAGE087
Represents the movement amount of the kinematic joint 8;
Figure 487182DEST_PATH_IMAGE088
represents the attitude transformation relationship between coordinate systems described by the improved DH method,
Figure 898572DEST_PATH_IMAGE089
, a,
Figure 805348DEST_PATH_IMAGE090
,
Figure 694807DEST_PATH_IMAGE091
Indicates the corresponding DH parameter;
Figure 885486DEST_PATH_IMAGE092
Represents the attitude transformation relationship between coordinate systems described by the Hayati model,
Figure 202197DEST_PATH_IMAGE093
represents the corresponding DH parameter; according to the geometric relationship between the adjacent joints of the footed robot in the embodiment, the forward kinematics model of the footed robot can be obtained as:

Figure 330690DEST_PATH_IMAGE094
Figure 330690DEST_PATH_IMAGE094

其中，函数

Figure 23840DEST_PATH_IMAGE095
表示将齐次变换矩阵
Figure 69025DEST_PATH_IMAGE096
转换为对应的欧拉角及位置坐标的函数；定 义对应的D-H参数如表1所示。可以看出，本机器每条腿共包含21个几何参数以及5个关节零 位参数，因此需要标定的参数总计为52个，即至少需要6个位形可以实现机器人的标定计 算。但是，一般情况下，为保证足够好的标定效果，用于标定的位形数目最好要远大于所需 要的最小数目，并尽量覆盖机器人的全部工作空间。 Among them, the function
Figure 23840DEST_PATH_IMAGE095
Represents a homogeneous transformation matrix
Figure 69025DEST_PATH_IMAGE096
It is converted into a function of the corresponding Euler angle and position coordinates; the corresponding DH parameters are defined as shown in Table 1. It can be seen that each leg of the machine contains a total of 21 geometric parameters and 5 joint zero position parameters, so the total number of parameters to be calibrated is 52, that is, at least 6 configurations are required to realize the calibration calculation of the robot. However, in general, in order to ensure a good enough calibration effect, the number of configurations used for calibration should preferably be much larger than the required minimum number, and try to cover the entire working space of the robot.

表1：实施例中用于运动学正解的D-H参数Table 1: D-H parameters for positive kinematic solutions in the examples

Figure 822218DEST_PATH_IMAGE097
Figure 822218DEST_PATH_IMAGE097

其中，

Figure 703586DEST_PATH_IMAGE098
分别表示第i条腿中身体到关节8，关节8到 关节9，关节9到关节10，关节10到关节12，关节12到关节15的位姿传递关系矩阵，下角标字 母即代表所采用的具体D-H建模方法，后续参数为对应方法中的参数，具体包括几何参数、 关节零位参数以及关节运动参数。 in,
Figure 703586DEST_PATH_IMAGE098
Represents the body to joint 8, joint 8 to joint 9, joint 9 to joint 10, joint 10 to joint 12, and joint 12 to joint 15 in the i -th leg. The matrix of the pose transfer relationship, the subscript letters represent the adopted For a specific DH modeling method, the subsequent parameters are the parameters in the corresponding method, and specifically include geometric parameters, joint zero position parameters, and joint motion parameters.

构建机器人第一及第二位姿误差模型的基础是获取相邻构件或运动关节坐标系 之间的误差传递关系，根据

Figure 731585DEST_PATH_IMAGE099
的定义，可以将其对应的位姿误差模型写作： The basis for constructing the first and second pose error models of the robot is to obtain the error transfer relationship between adjacent components or motion joint coordinate systems.
Figure 731585DEST_PATH_IMAGE099
The definition of , the corresponding pose error model can be written as:

Figure 382009DEST_PATH_IMAGE100
Figure 382009DEST_PATH_IMAGE100

其中，in,

Figure 555370DEST_PATH_IMAGE101
Figure 555370DEST_PATH_IMAGE101

表示对应的几何参数误差，

Figure 924035DEST_PATH_IMAGE102
表示关节零位偏差。几何、关节零位偏差与位姿误差 传递关系的具体表述可通过下式得到 represents the corresponding geometric parameter error,
Figure 924035DEST_PATH_IMAGE102
Indicates the joint zero deviation. The specific expression of the transfer relationship between geometry, joint zero deviation and pose error can be obtained by the following formula

Figure 958987DEST_PATH_IMAGE103
Figure 958987DEST_PATH_IMAGE103

在此基础上，足式机器人的第一位姿误差模型可以写作： On this basis, the first pose error model of the footed robot can be written as:

Figure 463917DEST_PATH_IMAGE104
Figure 463917DEST_PATH_IMAGE104

其中，in,

Figure 542601DEST_PATH_IMAGE105
Figure 542601DEST_PATH_IMAGE105

上式中，为6维单位矩阵，

Figure 971625DEST_PATH_IMAGE107
中的矩阵可根据所对应几何、关节零位偏差与位 姿误差传递关系矩阵得到。 In the above formula, is a 6-dimensional identity matrix,
Figure 971625DEST_PATH_IMAGE107
The matrix in can be obtained according to the corresponding geometry, joint zero deviation and pose error transfer relationship matrix.

根据上述结果，可得足14与足16之间的第二位姿误差传递模型为：According to the above results, the second pose error transfer model between the feet 14 and 16 can be obtained as:

Figure 596641DEST_PATH_IMAGE108
Figure 596641DEST_PATH_IMAGE108

将上述两种误差模型合并，即可得到用于进行足式机器人几何参数与关节零位自标定的误差模型：Combining the above two error models, the error model used for the self-calibration of the geometric parameters of the footed robot and the zero position of the joint can be obtained:

Figure 111805DEST_PATH_IMAGE109
Figure 111805DEST_PATH_IMAGE109

其中，误差模型中的各部分可以写作：Among them, each part in the error model can be written as:

Figure 189483DEST_PATH_IMAGE110
Figure 189483DEST_PATH_IMAGE110

步骤S103，基于所述的运动学正解模型、第一位姿误差模型和第二位姿误差模型，利用多组位形下足式机器人的姿态以及关节位置数据，对足式机器人几何参数与关节零位进行辨识计算，直至残差收敛至给定阈值或已收敛，最终得到的关节零位及几何参数即为标定结果。图5是根据一示例性实施例示出的几何参数与关节零位的辨识计算流程图。具体过程可参照图5所示并如下文所述：Step S103, based on the described kinematics positive solution model, the first pose error model and the second pose error model, using the posture and joint position data of the footed robot under multiple sets of configurations, the geometric parameters and joints of the footed robot are compared. The zero position is identified and calculated until the residual converges to the given threshold or has converged, and the final joint zero position and geometric parameters are the calibration results. FIG. 5 is a flow chart of identification and calculation of geometric parameters and joint zero positions according to an exemplary embodiment. The specific process can be referred to as shown in Figure 5 and described as follows:

（1）开始：完成准备工作，包括1）建立机器人的运动学正解模型，以及第一位姿误差模 型、第二位姿误差模型；2）将机器人左右足部分别固定于标定板的第一定位部3和第二定位 部5，并保证标定板相对于全局坐标系出于静止状态；3）使用惯性测量单元测得足部固定装 置的姿态向量

Figure 831817DEST_PATH_IMAGE111
，并通过设计图纸，得到标定板上足部固定装置2相对于1的位姿矢量
Figure 311340DEST_PATH_IMAGE112
； (1) Start: Complete the preparatory work, including 1) establishing the positive kinematics model of the robot, as well as the first pose error model and the second pose error model; 2) Fixing the left and right feet of the robot on the first position of the calibration board respectively Positioning part 3 and second positioning part 5, and ensure that the calibration plate is in a static state relative to the global coordinate system; 3) Use the inertial measurement unit to measure the attitude vector of the foot fixation device
Figure 831817DEST_PATH_IMAGE111
, and through the design drawings, the pose vector of the foot fixation device 2 relative to 1 on the calibration board is obtained
Figure 311340DEST_PATH_IMAGE112
;

（2）机器人名义几何及关节零位参数定义：确定机器人的理论（名义）几何与关节零位参数，该值也是机器人进行标定时参数辨识计算的初值；(2) Definition of the robot's nominal geometry and joint zero position parameters: determine the robot's theoretical (nominal) geometry and joint zero position parameters, which are also the initial values of the parameter identification calculation when the robot is calibrated;

（3）标定实验数据获取：控制足式机器人运动到不同位形，待静止后测得关节位置信息

Figure 544875DEST_PATH_IMAGE113
和身体姿态
Figure 353257DEST_PATH_IMAGE114
；根据本发明中所述的第一位姿误差模型及第二位姿误差模型，每个位形下 可以获得
Figure 268123DEST_PATH_IMAGE115
个约束方程，标定的基本要求是约束方程的数目不少于所要标定的几何 与关节零位参数数目； (3) Calibration experimental data acquisition: control the footed robot to move to different configurations, and measure the joint position information after it is stationary
Figure 544875DEST_PATH_IMAGE113
and body posture
Figure 353257DEST_PATH_IMAGE114
; According to the first pose error model and the second pose error model described in the present invention, each configuration can obtain
Figure 268123DEST_PATH_IMAGE115
The basic requirement of calibration is that the number of constraint equations is not less than the number of geometric and joint zero parameters to be calibrated;

（4）当前参数下的标定模型计算：通过机器人的运动学正解模型计算得到第m个位形下 两个足部相对于世界坐标系的位姿矢量

Figure 867732DEST_PATH_IMAGE116
；在此基础上，根据第一位姿误差模 型及第二位姿误差模型，可以得到用于标定迭代计算的模型，即： (4) Calculation of the calibration model under the current parameters: The pose vector of the two feet relative to the world coordinate system under the mth configuration is obtained by calculating the positive solution model of the robot's kinematics
Figure 867732DEST_PATH_IMAGE116
; On this basis, according to the first pose error model and the second pose error model, the model used for the calibration iterative calculation can be obtained, namely:

Figure 475430DEST_PATH_IMAGE118
Figure 475430DEST_PATH_IMAGE118

其中，下角标k表示此次模型处于第k次的迭代计算过程中，在第一次迭代时，模型中的几何及关节零位参数即为机器人的设计值。Among them, the subscript k indicates that the model is in the k -th iteration calculation process. In the first iteration, the geometry and joint zero position parameters in the model are the design values of the robot.

（5）偏差评价：根据计算得到偏差值，判断是否满足实际精度要求，如满足，则完成标定；若不满足，则进行参数辨识计算；(5) Deviation evaluation: According to the deviation value obtained by calculation, it is judged whether the actual accuracy requirements are met. If so, the calibration is completed; if not, the parameter identification calculation is carried out;

（6）参数辨识计算：(6) Parameter identification calculation:

Figure 511388DEST_PATH_IMAGE119
Figure 511388DEST_PATH_IMAGE119

（7）更新几何及关节零位参数：(7) Update geometry and joint zero position parameters:

Figure 229946DEST_PATH_IMAGE120
Figure 229946DEST_PATH_IMAGE120

（8）当前参数下机器人运动学正解计算：机器人几何及关节零位参数更新后，重新计算 所有标定位形下两个足部相对于世界坐标系的位姿矢量

Figure 684061DEST_PATH_IMAGE121
； (8) Calculation of the positive solution of the robot kinematics under the current parameters: After the robot geometry and joint zero position parameters are updated, recalculate the pose vectors of the two feet relative to the world coordinate system under all calibration positions
Figure 684061DEST_PATH_IMAGE121
;

（9）重复上述（4）-（8）过程，直至满足偏差评价要求或迭代计算已收敛，即完成整个标定过程，最终得到的几何与关节零位参数即为标定结果。(9) Repeat the above (4)-(8) process until the deviation evaluation requirements are met or the iterative calculation has converged, that is, the entire calibration process is completed, and the final geometry and joint zero parameters are the calibration results.

与前述的几何参数与关节零位自标定方法的实施例相对应，本申请还提供了一种几何参数与关节零位自标定装置的实施例。图6是根据一示例性实施例示出的一种几何参数与关节零位自标定装置的框图，该装置用于足式机器人，该装置包括：Corresponding to the foregoing embodiments of the geometric parameter and joint zero self-calibration method, the present application also provides an embodiment of a geometric parameter and joint zero self-calibration device. Fig. 6 is a block diagram showing a geometric parameter and joint zero position self-calibration device according to an exemplary embodiment, the device is used for a footed robot, and the device includes:

获取单元21，用于所述获取足式机器人身体的姿态以及关节位置数据，其中所述足式机器人被安装在标定板上；an acquisition unit 21 for acquiring the posture and joint position data of the footed robot body, wherein the footed robot is installed on a calibration plate;

模型构建单元22，用于根据所述姿态以及关节位置数据，分别构建足式机器人身体与足部之间的运动学正解模型、足式机器人身体到足部之间的第一位姿误差模型以及足式机器人各足部之间的第二位姿误差模型，其中所述运动学正解模型用于根据身体的姿态以及关节位置数据求解对应的足部位姿，所述第一位姿误差模型用于描述所对应的腿部连杆尺寸误差以及运动关节零位误差到足部位姿误差的传递关系；所述第二位姿误差模型用于描述不同足部之间的受对应腿部几何参数偏差以及关节零位偏差影响的位姿误差关系；The model construction unit 22 is used for constructing the positive kinematics model between the body and the foot of the footed robot, the first posture error model between the body and the foot of the footed robot, and The second pose error model between the feet of the footed robot, wherein the kinematics positive solution model is used to solve the corresponding foot pose according to the body pose and joint position data, and the first pose error model is used for Describe the corresponding leg link size error and the transfer relationship between the motion joint zero position error and the foot position error; the second position error model is used to describe the difference between different feet by the geometric parameter deviation of the corresponding leg and Pose error relationship affected by joint zero deviation;

计算单元23，用于基于所述的运动学正解模型、第一位姿误差模型和第二位姿误差模型，利用多组位形下足式机器人的姿态以及关节位置数据，对足式机器人几何参数与关节零位进行辨识计算，直至残差收敛至给定阈值，最终得到的关节零位及几何参数。The computing unit 23 is used for, based on the described kinematics positive solution model, the first pose error model and the second pose error model, to use the posture and joint position data of the footed robot under multiple groups of configurations to analyze the geometry of the footed robot. The parameters and joint zero position are identified and calculated until the residual converges to a given threshold, and the joint zero position and geometric parameters are finally obtained.

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。The above-mentioned serial numbers of the embodiments of the present invention are only for description, and do not represent the advantages or disadvantages of the embodiments.

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。In the above-mentioned embodiments of the present invention, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference may be made to related descriptions of other embodiments.

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的设备实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. The device embodiments described above are only illustrative, for example, the division of the units may be a logical function division, and there may be other division methods in actual implementation, for example, multiple units or components may be combined or Integration into another system, or some features can be ignored, or not implemented. On the other hand, the shown or discussed mutual coupling or direct coupling or communication connection may be through some interfaces, indirect coupling or communication connection of units or modules, and may be in electrical or other forms.

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。The units described as separate components may or may not be physically separated, and components shown as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution in this embodiment.

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units may be implemented in the form of hardware, or may be implemented in the form of software functional units.

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。The integrated unit, if implemented in the form of a software functional unit and sold or used as an independent product, may be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium , including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes: U disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), mobile hard disk, magnetic disk or optical disk and other media that can store program codes .

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the scope of the present invention. within the scope of protection.

Claims (10)

1. A geometric parameter and joint zero position self-calibration method is used for geometric parameter and joint zero position self-calibration of a foot robot, and is characterized by comprising the following steps:
acquiring posture and joint position data of the body of the legged robot, wherein the feet of the legged robot are fixed on a calibration plate;
respectively constructing a kinematic forward solution model between the body and the foot of the foot type robot, a first posture error model between the body and the foot of the foot type robot and a second posture error model between the feet of the foot type robot according to the posture and the joint position data, wherein the kinematic forward solution model is used for solving the corresponding foot posture according to the posture and the joint position data, and the first posture error model is used for describing the transmission relation between the corresponding leg connecting rod size error and the zero position error of the moving joint to the foot posture error; the second pose error model is used for describing pose error relations among different feet, which are influenced by geometric parameter deviations of corresponding legs and zero deviations of joints;
and identifying and calculating the geometric parameters and the zero positions of the joints of the foot type robot by using the postures and joint position data of the foot type robot under multiple groups of postures based on the kinematics positive solution model, the first posture error model and the second posture error model until the residual error converges to a given threshold value or the iterative calculation converges, and finally obtaining the zero positions and the geometric parameters of the joints.
2. The method for self-calibration of geometric parameters and zero positions of joints according to claim 1, wherein the calibration plate is provided with positioning parts, the number of which is the same as that of the feet of the legged robot, for connecting the feet of the legged robot and calibrating the position relation of the feet.
3. The method for zero position self-calibration of geometric parameters and joints according to claim 1, wherein the step of constructing the first position error model from the body to the foot of the legged robot comprises:
for each leg, according to the geometric dimension parameters of the leg component of the robot and the position information of the motion joint, constructing a transfer relation between the geometric dimension error of the leg component and the zero offset of the motion joint to the corresponding attitude offset of the foot, wherein the transfer relation is a first attitude error model.
4. The method for zero position self-calibration of geometric parameters and joints according to claim 1, wherein the step of constructing a second attitude error model between the feet of the legged robot comprises:
and obtaining the real pose relations of two preset feet through a calibration plate, simultaneously obtaining the theoretical pose relations of the two feet by utilizing the kinematics forward solution model, and constructing a transfer relation from the corresponding leg geometric parameter errors and the joint zero position errors to the foot pose errors by taking the minimum difference between the real pose and the theoretical pose of the two feet as an optimization target, wherein the transfer relation is a second pose error model.
5. A geometrical parameter and joint zero position self-calibration device, which is used for a foot type robot, is characterized by comprising:
an acquisition unit for acquiring posture and joint position data of a body of a legged robot mounted on a calibration board;
the model building unit is used for respectively building a kinematic positive solution model between the body and the foot of the foot type robot, a first position error model between the body and the foot of the foot type robot and a second position error model between the feet of the foot type robot according to the posture and the joint position data, wherein the kinematic positive solution model is used for solving the corresponding foot position and posture according to the posture of the body and the joint position data, and the first position error model is used for describing the transfer relationship from the corresponding leg connecting rod size error and the zero position error of the moving joint to the foot position and posture error; the second pose error model is used for describing pose error relations among different feet, which are influenced by geometric parameter deviations of corresponding legs and zero deviations of joints;
and the calculation unit is used for identifying and calculating the geometric parameters and the joint zero position of the foot type robot by utilizing the postures and the joint position data of the foot type robot under multiple groups of posture shapes based on the kinematics positive solution model, the first posture error model and the second posture error model until the residual error converges to a given threshold value, and finally obtaining the joint zero position and the geometric parameters.
6. The device for automatically calibrating geometric parameters and zero positions of joints according to claim 5, wherein the calibration plate is provided with positioning parts which are as many as the feet of the legged robot, are used for connecting the feet of the legged robot, and calibrate the position relation of the feet.
7. The device for self-calibration of geometric parameters and zero position of joints according to claim 5, wherein constructing a first position error model between the body and the foot of the legged robot comprises:
for each leg, according to the geometric dimension parameters of the leg component of the robot and the position information of the motion joint, constructing a transfer relation between the geometric dimension error of the leg component and the zero offset of the motion joint to the corresponding attitude offset of the foot, wherein the transfer relation is a first attitude error model.
8. The device for self-calibration of geometric parameters and zero position of joints according to claim 5, wherein constructing a second model of attitude errors between feet of the legged robot comprises:
and obtaining the real pose relations of two preset feet through a calibration plate, simultaneously obtaining the theoretical pose relations of the two feet by utilizing the kinematics forward solution model, and constructing a transfer relation from the corresponding leg geometric parameter errors and the joint zero position errors to the foot pose errors by taking the minimum difference between the real pose and the theoretical pose of the two feet as an optimization target, wherein the transfer relation is a second pose error model.
9. An electronic device, comprising:
one or more processors;
a memory for storing one or more programs;
when executed by the one or more processors, cause the one or more processors to implement the method of any one of claims 1-4.
10. A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the method according to any one of claims 1-4.
CN202011560013.6A 2020-12-25 2020-12-25 Geometric parameter and joint zero position self-calibration method and device, electronic equipment and medium Active CN112276959B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011560013.6A CN112276959B (en) 2020-12-25 2020-12-25 Geometric parameter and joint zero position self-calibration method and device, electronic equipment and medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011560013.6A CN112276959B (en) 2020-12-25 2020-12-25 Geometric parameter and joint zero position self-calibration method and device, electronic equipment and medium

Publications (2)

Publication Number Publication Date
CN112276959A true CN112276959A (en) 2021-01-29
CN112276959B CN112276959B (en) 2021-03-30

Family

ID=74426287

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011560013.6A Active CN112276959B (en) 2020-12-25 2020-12-25 Geometric parameter and joint zero position self-calibration method and device, electronic equipment and medium

Country Status (1)

Country Link
CN (1) CN112276959B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113345030A (en) * 2021-06-17 2021-09-03 三一重机有限公司 Calibration method and device for sensor of working machine, working machine and electronic equipment

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101763118A (en) * 2002-03-18 2010-06-30 索尼株式会社 Robot device, device and method for controlling operation of legged locomotion robot, sensor system for legged locomotion robot, and moving body device
US20130131865A1 (en) * 2011-11-22 2013-05-23 Disney Enterprises, Inc. Kinematic and Dynamic Calibration Methods for Legged Robots with Force-Controlled Joints
CN105808882A (en) * 2016-03-29 2016-07-27 郑州轻工业学院 Calibration method and device for movement parameters of reptile-imitated four-foot walking robot
US9778132B1 (en) * 2015-12-16 2017-10-03 X Development Llc Methods and systems for force sensor calibration
CN208898448U (en) * 2018-09-29 2019-05-24 浙江迈睿机器人有限公司 A kind of calibrating device suitable for two-wheel differential fork truck
US20200189110A1 (en) * 2017-06-29 2020-06-18 Sony Interactive Entertainment Inc. Robot control apparatus, control method and control program

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101763118A (en) * 2002-03-18 2010-06-30 索尼株式会社 Robot device, device and method for controlling operation of legged locomotion robot, sensor system for legged locomotion robot, and moving body device
US20130131865A1 (en) * 2011-11-22 2013-05-23 Disney Enterprises, Inc. Kinematic and Dynamic Calibration Methods for Legged Robots with Force-Controlled Joints
US9778132B1 (en) * 2015-12-16 2017-10-03 X Development Llc Methods and systems for force sensor calibration
CN105808882A (en) * 2016-03-29 2016-07-27 郑州轻工业学院 Calibration method and device for movement parameters of reptile-imitated four-foot walking robot
US20200189110A1 (en) * 2017-06-29 2020-06-18 Sony Interactive Entertainment Inc. Robot control apparatus, control method and control program
CN208898448U (en) * 2018-09-29 2019-05-24 浙江迈睿机器人有限公司 A kind of calibrating device suitable for two-wheel differential fork truck

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113345030A (en) * 2021-06-17 2021-09-03 三一重机有限公司 Calibration method and device for sensor of working machine, working machine and electronic equipment
CN113345030B (en) * 2021-06-17 2024-01-02 三一重机有限公司 Calibration method and device for working machine sensor, working machine and electronic equipment

Also Published As

Publication number Publication date
CN112276959B (en) 2021-03-30

Similar Documents

Publication Publication Date Title
CN107738254B (en) A method and system for converting and calibrating a manipulator coordinate system
Kofinas et al. Complete analytical forward and inverse kinematics for the NAO humanoid robot
CN104596502B (en) Object posture measuring method based on CAD model and monocular vision
Zhuang Self-calibration of parallel mechanisms with a case study on Stewart platforms
Kofinas et al. Complete analytical inverse kinematics for NAO
CN111168719B (en) Robot calibration method and system based on positioning tool
TWI672207B (en) Posture positioning system for machine and the method thereof
KR20160070006A (en) Collision avoidance method, control device, and program
CN112720483A (en) Method and device for acquiring combined mass center state, humanoid robot and readable storage medium
Xiao et al. A new fixed axis-invariant based calibration approach to improve absolute positioning accuracy of manipulators
WO2019114631A1 (en) Method and device for acquiring tcp coordinates of robot
CN114131595A (en) Robot 6D pose estimation system and method
CN116277035B (en) Robot control method and device, processor and electronic equipment
Liu et al. Kinematic calibration of a six-legged walking machine tool
Santolaria et al. Self-alignment of on-board measurement sensors for robot kinematic calibration
CN112276959B (en) Geometric parameter and joint zero position self-calibration method and device, electronic equipment and medium
WO2024193002A1 (en) Multi-degree-of-freedom mechanism kinematics modeling method, apparatus and device, and storage medium
CN113119102B (en) Humanoid robot modeling method and device based on floating-base flywheel inverted pendulum
CN114654466B (en) Automatic calibration method, device, system, electronic equipment and storage medium
CN112894814B (en) Mechanical arm DH parameter identification method based on least square method
CN113589817A (en) Foot-type robot foot-falling control method and device, electronic equipment and storage medium
CN113253748A (en) State estimation method, device, equipment and medium for parallel humanoid robot
CN113465822B (en) Centroid position calculation method, device and medium based on multi-rigid robot
CN116077190B (en) System for improving accuracy of surgical robot system
CN114474069B (en) Robot line structure light hand-eye calibration method based on space orthogonal constraint

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