关节位姿的校准方法、系统及存储介质与流程

1.本技术涉及工业机器人技术领域，尤其涉及一种关节位姿的校准方法、系统及存储介质。


背景技术：

2.码垛机器人通过运用平行四边形机构，使得机器人末端中心点只在x、y、z方向移动，其具有运作灵活精准、稳定性高、作业效率高等优点。但由于现有的制造工艺的局限性，码垛机器人的运动控制过程中仍存在着误差，且大多码垛机器人并没有将这些误差因素纳入考虑范围内，影响了码垛机器人运动的控制精度。


技术实现要素：

3.本技术旨在至少解决现有技术中存在的技术问题之一。为此，本技术提出一种关节位姿的校准方法、系统及存储介质，能够有效提高码垛机器人的控制精度。
4.为解决上述技术问题，本发明提出如下技术方案：
5.本技术第一方面实施例提供了一种关节位姿的校准方法，应用于码垛机器人，包括：
6.采集所述码垛机器人的关节位姿，对所述关节位姿进行计算，得到若干个第一关节空间向量和若干个第一测量位姿；
7.根据所述码垛机器人的结构得到运动学模型函数和第一校准参数，将所述第一关节空间向量输入到所述运动学模型函数，得到第一名义位姿；
8.根据所述第一测量位姿和所述第一名义位姿计算得到第二名义位姿，对所述第一名义位姿和所述第二名义位姿进行计算，得到位置坐标误差；
9.根据所述第一校准参数计算得到所述第一校准参数对应的雅克比矩阵，根据所述雅克比矩阵和所述位置坐标误差计算得到目标校准参数；
10.更新所述第一关节空间向量，重新将更新的所述第一关节空间向量输入到所述运动学模型函数，以更新所述目标校准参数，直至满足校准条件，其中，满足所述校准条件的目标校准参数用于校准所述码垛机器人的关节位姿。
11.根据本技术第一方面实施例的关节位姿的校准方法，至少具有如下有益效果：本技术的关节位姿的校准方法通过分析码垛机器人的结构组成，抽象出了码垛机器人各关节的旋转副的角度关系，并根据码垛机器人的结构得到运动学模型函数和第一校准参数，成功建立了码垛机器人的正解模型，确定了影响码垛机器人运动精度的重要参数，实现了码垛机器人的结构参数校准，有效提高了码垛机器人的控制精度。
12.根据本技术第一方面的一些实施例，所述根据所述码垛机器人的结构得到运动学模型函数和第一校准参数，包括：
13.根据所述码垛机器人的结构建立连杆坐标系；
14.根据所述连杆坐标系得到所述码垛机器人的连杆对应的dh参数和第一校准参数，
其中所述dh参数包括连杆参数、连杆扭角、连杆偏距和连杆关节角，所述第一校准参数包括连杆参数、减速比修正系数、零位修正系数和工具坐标；
15.根据所述dh参数和所述第一校准参数构建运动学模型函数。
16.根据本技术第一方面的一些实施例，所述根据所述dh参数和所述第一校准参数构建运动学模型函数，包括：
17.根据所述dh参数构建dh连杆转换矩阵；
18.根据所述dh连杆转换矩阵和所述工具坐标，计算得到所述码垛机器人末端的位姿矩阵；
19.从所述位姿矩阵中提取得到运动学模型函数。
20.根据本技术第一方面的一些实施例，所述根据所述第一测量位姿和所述第一名义位姿计算得到第二名义位姿，包括：
21.根据所述第一测量位姿和所述第一名义位姿，使用svd方法计算得到位姿转换矩阵；
22.对所述位姿转换矩阵和所述第一测量位姿执行相乘操作，得到第二名义位姿。
23.根据本技术第一方面的一些实施例，所述根据所述第一测量位姿和所述第一名义位姿，使用svd方法计算得到位姿转换矩阵，包括：
24.分别计算所述第一测量位姿构成的第一点集的重心和所述第一名义位姿构成的第二点集的重心；
25.对所述第一点集的重心和所述第二点集的重心执行重合操作，得到所述第一点集的重心相对于所述第二点集的重心的第一相对坐标点集，和所述第二点集的重心相对于所述第一点集的重心的第二相对坐标点集；
26.根据所述第一相对坐标点集和所述第二相对坐标点集构建协矩阵，对所述协矩阵进行svd分解，根据分解后的协矩阵，计算得到位姿转换矩阵。
27.根据本技术第一方面的一些实施例，所述根据所述雅克比矩阵和所述位置坐标误差计算得到目标校准参数，包括：
28.根据所述雅克比矩阵、所述位置坐标误差和预设的更新系数，计算得到海森矩阵和误差向量；
29.根据所述海森矩阵和所述误差向量计算得到目标校准参数。
30.根据本技术第一方面的一些实施例，所述更新所述第一关节空间向量，包括以下之一：
31.当预设的初始更新次数等于第一更新阈值，更新所述第一关节空间向量；或者，
32.当所述初始更新次数大于所述第一更新阈值且小于第二更新阈值，且当前位置坐标误差的有效值小于上一位置坐标误差的有效值，对所述更新系数执行缩小操作后，更新所述第一关节空间向量；或者，
33.当所述初始更新次数大于所述第一更新阈值且小于所述第二更新阈值，且当前位置坐标误差的有效值大于上一位置坐标误差的有效值，对所述更新系数执行放大操作后，更新所述第一关节空间向量。
34.根据本技术第一方面的一些实施例，所述校准条件包括以下之一：
35.所述初始更新次数等于所述第二更新阈值；或者
36.所述初始更新次数大于所述第一更新阈值且小于所述第二更新阈值，且当前位置坐标误差的有效值对应上一位置坐标误差的有效值的差小于或等于第二向量阈值；或者，
37.所述初始更新次数大于所述第一更新阈值且小于所述第二更新阈值，且所述误差向量的模小于或等于第一向量阈值。
38.本技术第二方面实施例提供了一种关节位姿的校准系统，包括：
39.至少一个存储器；
40.至少一个处理器；
41.至少一个程序；
42.所述程序被存储在所述存储器中，所述处理器执行至少一个所述程序以实现如本技术第一方面任一项实施例所述的关节位姿的校准方法。
43.本技术第三方面实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行信号，所述计算机可执行信号用于执行如本技术第一方面任一项实施例所述的关节位姿的校准方法。
44.本技术的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本技术的实践了解到。
附图说明
45.本技术的附加方面和优点结合下面附图对实施例的描述中将变得明显和容易理解，其中：
46.图1为本技术一实施例提供的码垛机器人的结构示意图；
47.图2为本技术一实施例提供的码垛机器人的连杆坐标系的示意图；
48.图3为本技术一实施例提供的关节位姿的校准方法的流程图；
49.图4为本技术一实施例提供的构建运动学模型函数的流程图；
50.图5为本技术另一实施例提供的构建运动学模型函数的流程图；
51.图6为本技术一实施例提供的根据第一测量位姿和第一名义位姿计算得到第二名义位姿的流程图；
52.图7为本技术一实施例提供的根据第一测量位姿和第一名义位姿，使用svd方法计算得到位姿转换矩阵的流程图；
53.图8为本技术一实施例提供的根据雅克比矩阵和位置坐标误差得到目标校准参数的流程图；
54.图9为本技术一实施例提供的关节位姿的校准系统的模块框图。
55.附图标记：底座100；转座200；大臂300；大臂拉杆310；三角架结构400；小臂500；小臂拉杆510；吊头600；末端法兰700；处理器800；存储器900。
具体实施方式
56.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
57.需要说明的是，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同
于流程图中的顺序执行所示出或描述的步骤。说明书和权利要求书及上述附图中的术语等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
58.在本技术的描述中，如果有描述到第一、第二只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。
59.本技术的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本技术中的具体含义。
60.参照图1，图1为本技术一实施例提供的码垛机器人的结构示意图；可以理解的是，码垛机器人的结构包括：底座100、转座200、大臂拉杆310、三角架结构400、小臂拉杆510、吊头600、末端法兰700、小臂500和大臂300；底座100用于固定本技术的码垛机器人，使得码垛机器人在工作过程中不与地面发生相对移动，转座200与底座100连接，用于驱动大臂300和大臂拉杆310在工作空间内做前后摆动运动，大臂300和大臂拉杆310平行，且大臂300的一端、大臂拉杆310的一端都与三角架结构400连接，三角架结构400用于调整小臂500和小臂拉杆510的方向、姿态，方便控制小臂500、小臂拉杆510在工作空间内运动，小臂500的另一端与吊头600、末端法兰700连接，用于控制吊头600和末端法兰700的方向和位置，末端法兰700用于连接其他物体，方便本技术的码垛机器人在工作过程中能适应不同形状的货物。在另一方面，本技术的码垛机器人通过采用平行四边形连杆机构灵活地实现了末端法兰700在水平方向和竖直方向上的移动，并结合了三角架结构400调节了姿态，使得末端法兰700实现了无耦合线性运动，可更灵活地活动并对货物实现抓取。
61.参照图3，第一方面，本技术实施例提供了一种关节位姿的校准方法，包括但不限于步骤s110、步骤s120、步骤s130、步骤s140、步骤s150。
62.步骤s110，采集码垛机器人的关节位姿，对关节位姿进行计算，得到若干个第一关节空间向量和若干个第一测量位姿；
63.可以理解的是，在采集码垛机器人的关节位姿之前，需先架设好码垛机器人和激光跟踪仪的位置，并将靶球固定于码垛机器人的末端法兰700的合适位置处，使得激光跟踪仪能够采集到码垛机器人的关节位姿。在架设好码垛机器人和激光跟踪仪的位置后，用关节空间向量示教机器人生成若干个不同的关节位姿，可以理解的是，生成的关节位姿的数量大于等于50组，且相邻两组的关节位姿对应的关节角元素变化大于15度。
64.在采集完码垛机器人的关节位姿后，需要将关节位姿分解为两组参数，分别为第一关节空间向量和第一测量位姿，且第一关节空间向量的数量、第一测量位姿的数量和关节位姿的数量相等。
65.步骤s120，根据码垛机器人的结构得到运动学模型函数和第一校准参数，将第一关节空间向量输入到运动学模型函数，得到第一名义位姿；
66.可以理解的是，本技术的关节位姿的校准方法通过分析码垛机器人的结构组成，抽象出了码垛机器人各关节的旋转副的角度关系，并基于码垛机器人各关节的旋转副的角度关系分别构建多个坐标系，进行dh参数分析，建立了码垛机器人的正解模型，得到了第一校准参数，再由dh参数得到运动学模型函数，确定了影响码垛机器人运动精度的重要参数。
67.需要说明的是，运动学模型函数为设于末端法兰700的靶球中心在码垛机器人底
座100坐标系下的空间坐标，即空间位姿，是3*1的向量，将第一关节空间向量输入到运动学模型函数中，可得到第一名义位姿。
68.步骤s130，根据第一测量位姿和第一名义位姿计算得到第二名义位姿，对第一名义位姿和第二名义位姿进行计算，得到位置坐标误差；
69.可以理解的是，由于第一测量位姿是由激光跟踪仪得到的，第一名义位姿是基于运动学模型函数计算得到的码垛机器人的末端位姿，为了构建激光跟踪仪与机器人末端位姿的映射关系，需通过svd法求得由第一测量位姿到第一名义位姿的转换矩阵，即激光跟踪仪的转换矩阵，再基于转换矩阵求得第二名义位姿，并计算出第一名义位姿和第二名义位姿之间的位置坐标误差，需要说明的是，若码垛机器人不存在制造和装配误差，位置坐标误差的数值应为0。
70.步骤s140，根据第一校准参数计算得到第一校准参数对应的雅克比矩阵，根据雅克比矩阵和位置坐标误差计算得到目标校准参数；
71.可以理解的是，本技术的关节位姿的校准方法结合了levenberg-marquard算法，持续对目标校准参数迭代更新，而在levenberg-marquard算法的校准过程中，需要求得第一校准参数的导数，并根据第一校准参数的导数建立雅克比矩阵，在构建完雅克比矩阵后，需结合雅克比矩阵和位置坐标误差计算得到海森矩阵和误差向量，继而由海森矩阵和误差向量得到目标校准参数。
72.步骤s150，更新第一关节空间向量，重新将更新的第一关节空间向量输入到运动学模型函数，以更新目标校准参数，直至满足校准条件，其中，满足校准条件的目标校准参数用于校准码垛机器人的关节位姿。
73.可以理解的是，在levenberg-marquard算法的迭代更新过程中，海森矩阵和误差向量一直在迭代更新，因而目标校准参数也随着海森矩阵、误差向量在迭代更新，只有在满足校准条件，才能停止迭代更新，得到最终的目标校准参数。因此在满足校准条件之前，需对第一关节空间向量进行持续地迭代更新，并重新将迭代更新的第一关节空间向量输入到运动学模型函数，以更新目标校准参数。其中，最终的目标校准参数，即满足校准条件的目标校准参数用于校准码垛机器人的关节位姿，提高了码垛机器人的控制精度。
74.参照图4，第一方面，本技术实施例提供了一种构建运动学模型函数的方法，包括但不限于步骤s210、步骤s220、步骤s230。
75.步骤s210，根据码垛机器人的结构建立连杆坐标系；
76.参照图2，图2为本技术一实施例提供的码垛机器人的连杆坐标系的示意图，可以理解的是，本技术的关节位姿的校准方法通过分析码垛机器人的结构组成，抽象出了码垛机器人各关节的旋转副的角度关系，并基于码垛机器人各关节的旋转副的角度关系分别构建多个连杆坐标系。
77.具体的是，本技术构建的连杆坐标系均为右手坐标系，其中一轴和底座100底部交点为码垛机器人的基础坐标系，基础坐标系的z0轴沿一轴向上，x0轴向右，二轴垂直于一轴，连杆坐标系一的z1轴沿二轴轴线向外，x1轴沿z0轴和z1轴的的公垂线向右，三轴垂直于二轴，并与一轴平行，连杆坐标系二的z2轴沿三轴向外，x2轴与z2轴垂直且方向向上，四轴垂直于三轴，并与二轴平行，连杆坐标系三的z3轴沿四轴方向向外，x3轴垂直于z3轴且方向向右，连杆坐标系四的z4轴向上，x4轴垂直于z4轴向右，五轴垂直于四轴，并与三轴平行，末
端连杆坐标系五的z5轴沿五轴方向向上，x5轴的方向向右。工具坐标系的zt轴的方向与末端连杆坐标系五的z5轴的方向相同，xt轴的方向与末端连杆坐标系五的x5轴的方向相同，且工具坐标系位于码垛机器人的末端法兰700，即激光跟踪仪靶球的中心位置。
78.可以理解的是，在工程人员对码垛机器人的设计和装配中，一轴和五轴的轴线在竖直方向上都存在着一个偏移量，而一般的码垛机器人常常忽略了一轴和五轴在竖直方向的偏移量，本技术的关节位姿的校准方法通过将一轴和五轴的偏移量纳入了考虑范围内，提高了码垛机器人的控制精度，可将码垛机器人的绝对精度提高到0.5毫米以内。
79.步骤s220，根据连杆坐标系得到码垛机器人的连杆对应的dh参数和第一校准参数，其中dh参数包括连杆参数、连杆扭角、连杆偏距和连杆关节角，第一校准参数包括连杆参数、减速比修正系数、零位修正系数和工具坐标；
80.可以理解的是，码垛机器人存在着5个转动副，由于大臂300、大臂拉杆310和小臂500、小臂拉杆510两个平行四边形机构的运用，并通过了三角架结构400连接了这两个平行四边形机构，第四关节变为第二关节和第三关节的联动轴，因而码垛机器人的自由度为4，且二轴、三轴和四轴的关节角存在着q2 q3 q4＝0的特殊关系。
81.具体的是，码垛机器人的连杆对应的dh参数和第一校准参数如表ⅰ所示；表i如下表所示：
[0082][0083]
其中ai表示连杆参数，αi表示连杆扭角，di表示连杆偏距，qi表示连杆关节角，t
x t
y tz表示工具坐标，由表可知码垛机器人的连杆1-6对应的dh参数和第一校准参数，其中dh参数包括连杆参数、连杆扭角、连杆偏距和连杆关节角，第一校准参数包括连杆参数、减速比修正系数、零位修正系数和工具坐标。
[0084]
步骤s230，根据dh参数和第一校准参数构建运动学模型函数。
[0085]
可以理解的是，dh建模要解决的根本问题就是码垛机器人末端法兰700和底座100位姿关系的问题，dh法的实施步骤是先辨认出码垛机器人的关节和连杆，再确立z轴、x轴、y轴、每个连杆坐标系的原点，以确定dh参数和第一校准参数，最后根据dh参数建立dh连杆转换矩阵，本技术的关节位姿的校准方法结合了dh连杆转换矩阵和第一校准参数构建了运动学模型函数。
[0086]
参照图5，第一方面，本技术另一实施例提供了一种构建运动学模型函数的方法，包括但不限于步骤s310、步骤s320、步骤s330。
[0087]
步骤s310，根据dh参数构建dh连杆转换矩阵；
[0088]
可以理解的是，根据dh参数可以构建出dh连杆转换矩阵，以建立码垛机器人的末端法兰700和底座100位姿的关系，即激光跟踪仪靶球的中心位置与底座100位姿的关系，dh连杆转换矩阵为：
[0089][0090]
其中，ai表示连杆坐标系i的dh连杆转换矩阵，θi表示连杆的关节角变量，αi表示连杆扭角，di表示连杆偏距，transz表示基于z轴方向将参数输入trans函数，trans
x
表示基于x轴方向将参数输入trans函数，rotz表示基于z轴方向将参数输入rot函数，rot
x
表示基于x轴方向将参数输入rot函数。
[0091]
步骤s320，根据dh连杆转换矩阵和工具坐标，计算得到码垛机器人末端的位姿矩阵；
[0092]
需要说明的是，可根据dh连杆转换矩阵得到码垛机器人末端测量靶球中心的位姿，码垛机器人末端测量靶球中心的位姿为：
[0093][0094]
其中，t表示4
×
4的位姿矩阵，ai表示连杆坐标系i的dh连杆转换矩阵，tool表示工具坐标矩阵，r3×3表示3
×
3的单位矩阵，p3×1表示机器人末端的位姿信息，即本技术的运动学模型函数，是3
×
1的向量。
[0095]
需要说明的是，工具坐标矩阵为：
[0096][0097]
其中，tool表示工具坐标矩阵，t
x t
y tz表示工具坐标。
[0098]
步骤s330，从位姿矩阵中提取得到运动学模型函数；
[0099]
需要说明的是，运动学模型函数是从位姿矩阵中提取得到，为位姿矩阵第4列的前3行，运动学模型函数为：
[0100]
f(θ，η)＝[x y z]
t
＝p3×1[0101]
其中，θ＝[θ
1 θ
2 θ
3 θ4]表示4个关节角变量，η表示第一校准参数，一共为18个参数。
[0102]
需要说明的是，可以理解的是，本技术的关节位姿的校准方法结合了levenberg-marquard算法，持续对目标校准参数迭代更新，而在levenberg-marquard算法的校准过程中，需要求第一校准参数的导数，第一校准参数包括连杆坐标系1a1、连杆坐标系2a2、连杆坐标系3a3、连杆坐标系4a4、连杆坐标系5a5的校准参数，参照表ⅰ，连杆坐标系1a1、连杆坐标系2a2、连杆坐标系3a3、连杆坐标系4a4、连杆坐标系5a5的校准参数不同，所以连杆坐标系1a1、
连杆坐标系2a2、连杆坐标系3a3、连杆坐标系4a4、连杆坐标系5a5校准参数的导数也不同；
[0103]
其中，连杆坐标系1a1、连杆坐标系2a2、连杆坐标系3a3校准参数的导数为：
[0104][0105]
其中，j表示ai的校准参数的导数集，ji表示ai的校准参数的导数，ai表示连杆坐标系i的dh连杆转换矩阵，ηk表示连杆坐标系i的校准参数，tool表示工具坐标矩阵，j
tx
表示ai的工具坐标矩阵相对于工具坐标t
x
的导数，j
ty
表示ai的工具坐标矩阵相对于工具坐标ty的导数，j
tz
表示ai的工具坐标矩阵相对于工具坐标tz的导数。
[0106]
可以理解的是，连杆坐标系4a4的校准参数包括连杆坐标系2a2的校准参数k2、θ
o2
和连杆坐标系3a3的校准参数k3、θ
o3
，需分别对连杆坐标系2a2的校准参数k2、θ
o2
和连杆坐标系3a3的校准参数k3、θ
o3
求导，则连杆坐标系4a4的校准参数的导数为：
[0107][0108][0109]
其中，j
k2
表示连杆坐标系4a4的校准参数k2的导数，j
k3
表示连杆坐标系4a4的校准参数k3的导数，j
θo2
表示连杆坐标系4a4的校准参数θ
o2
的导数，j
θo3
表示连杆坐标系4a4的校准参数θ
o3
的导数。
[0110]
可以理解的是，连杆坐标系5a5的校准参数的导数为：
[0111][0112]
其中，表示ai的校准参数的导数，ai表示连杆坐标系i的dh连杆转换矩阵，ηk表示连杆坐标系i的校准参数，tool表示工具坐标矩阵，j
tx
表示ai的工具坐标矩阵相对于工具坐标t
x
的导数，j
ty
表示ai的工具坐标矩阵相对于工具坐标ty的导数，j
tz
表示ai的工具坐标矩阵相对于工具坐标tz的导数。
[0113]
可以理解的是，在使用levenberg-marquard算法的校准过程中，还需要根据第一校准参数的导数建立雅克比矩阵，第一校准参数的雅克比矩阵为：
[0114][0115]
其中，jn表示雅克比矩阵，j
x
表示第一校准参数相对于x轴的导数，jy表示第一校准参数相对于y轴的导数，jz表示第一校准参数相对于z轴的导数，f表示第一名义位姿，f
x
表示第一名义位姿在连杆坐标系的x轴的数值，fy表示第一名义位姿在连杆坐标系的y轴的数值，fz表示第一名义位姿在连杆坐标系的z轴的数值，ηn表示第一校准参数。
[0116]
参照图6，第一方面，本技术实施例提供了一种根据第一测量位姿和第一名义位姿计算得到第二名义位姿的方法，包括但不限于步骤s410、步骤s420。
[0117]
步骤s410，根据第一测量位姿和第一名义位姿，使用svd方法计算得到位姿转换矩阵；
[0118]
需要说明的是，第一测量位姿是基于激光跟踪仪采集的坐标系统采集得到的位姿，第一名义位姿在第一校准参数理想的情况下，基于码垛机器人底座100的坐标系统得到的，为比较第一测量位姿和第一名义位姿，需根据svd方法，计算得到由第一测量位姿到第一名义位姿的位姿转换矩阵，其中svd方法为奇异值分解法，用于分解矩阵。
[0119]
步骤s420，对位姿转换矩阵和第一测量位姿执行相乘操作，得到第二名义位姿。
[0120]
可以理解的是，第一测量位姿是基于激光跟踪仪采集的坐标系统采集得到的位姿，第一名义位姿在第一校准参数理想的情况下，基于码垛机器人底座100的坐标系统得到的，第二名义位姿结合了第一测量位姿到第一名义位姿的转换矩阵和第一测量位姿，既考虑了激光跟踪仪采集的坐标系统，也将码垛机器人底座100的坐标系统纳入在考虑范围内。
[0121]
需要说明的是，第二名义位姿为：
[0122]
pr＝t*p
t
[0123]
其中，pr表示第二名义位姿，p
t
表示第一测量位姿，t表示位姿转换矩阵。
[0124]
参照图7，第一方面，本技术实施例提供了一种根据第一测量位姿和第一名义位姿，使用svd方法计算得到位姿转换矩阵的方法，包括但不限于步骤s510、步骤s520、步骤s530。
[0125]
步骤s510，分别计算第一测量位姿构成的第一点集的重心和第一名义位姿构成的第二点集的重心；
[0126]
需要说明的是，位姿转换矩阵体现了激光跟踪仪与码垛机器人末端位置的映射关系，为了求得激光跟踪仪与码垛机器人末端位置的映射关系，本技术的关节位姿的校准方法使用svd方法求得位姿转换矩阵。首先需分别计算第一测量位姿构成的第一点集的重心和第一名义位姿构成的第二点集的重心，第一点集的重心即第一点集包含的全部点的坐标的平均值，第二点集的重心即第二点集包含的全部点的坐标的平均值。
[0127]
第一测量位姿构成的第一点集为：
[0128]
[0129]
其中，p
t
表示第一测量位姿构成的第一点集，p
tn
表示第一测量位姿，n表示第一测量位姿的数量。
[0130]
第一测量位姿构成的第一点集的重心为：
[0131][0132]
其中，μ
t
表示第一测量位姿构成的第一点集的重心，p
ri
表示第一测量位姿，n表示第一测量位姿的数量。
[0133]
第一名义位姿构成的第二点集为：
[0134][0135]
其中，pr表示第一名义位姿构成的第二点集，p
rn
表示第一名义位姿。
[0136]
第一名义位姿构成的第二点集的重心为：
[0137][0138]
其中，μr表示第一名义位姿构成的第二点集的重心，p
ri
表示第一名义位姿，n表示第一名义位姿的数量。
[0139]
步骤s520，对第一点集的重心和第二点集的重心执行重合操作，得到第一点集的重心相对于第二点集的重心的第一相对坐标点集，和第二点集的重心相对于第一点集的重心的第二相对坐标点集；
[0140]
需要说明的是，在构造协矩阵之前需要将第一点集和第二点集的重心对齐重合，并分别计算各点集相对重心的相对坐标构成的新的点集。
[0141]
第一点集的重心相对于第二点集的重心的第一相对坐标点集为：
[0142]
记：为：
[0143]
其中，表示第一点集的重心相对于第二点集的重心的第一相对坐标点集，p
ti
表示第一测量位姿，μ
t
表示第一测量位姿构成的第一点集的重心。
[0144]
第二点集的重心相对于第一点集的重心的第二相对坐标点集为：
[0145]
记：为：
[0146]
其中，表示第二点集的重心相对于第一点集的重心的第二相对坐标点集,p
ri
表示第一名义位姿，μr表示第一名义位姿构成的第二点集的重心。
[0147]
步骤s530，根据第一相对坐标点集和第二相对坐标点集构建协矩阵，对协矩阵进行svd分解，根据分解后的协矩阵，计算得到位姿转换矩阵。
[0148]
需要说明的是，根据第一相对坐标点集和第二相对坐标点集可构建协矩阵，协矩阵为：
[0149]
t3×1＝μ
r-rμ
t
[0150]
其中，t3×1表示协矩阵，协矩阵又名平移矩阵，μr表示第一名义位姿构成的第二点集的重心，r表示旋转矩阵，μ
t
表示第一测量位姿构成的第一点集的重心。
[0151]
对协矩阵进行svd分解后，得到分解后的协矩阵，分解后的协矩阵为：
[0152][0153]
其中，∑3×3表示分解后的协矩阵，表示第一相对坐标点集，表示第二相对坐标点集，u和v为旋转矩阵的参数，旋转矩阵即为r3×3＝vu
t
。
[0154]
根据协矩阵和旋转矩阵可得出第一测量位姿到第一名义位姿的位姿转换矩阵，位姿转换矩阵为：
[0155][0156]
其中，t表示位姿转换矩阵，r3×3表示旋转矩阵，t3×1表示协矩阵。
[0157]
需要说明的是，第一测量位姿减去第二名义位姿的差为位置坐标误差，如果码垛机器人不存在制造和装配误差，位置坐标误差的值为零，位置坐标误差为：
[0158]
l＝p
r-pn[0159]
其中，l表示位置坐标误差，pr表示第一测量位姿，pn表示第二名义位姿。
[0160]
可以理解的是，位置坐标误差也为第一测量位姿的点集和第二名义位姿的点集中的对应点的距离，则第一测量位姿的点集和第二名义位姿的点集中的对应点的距离的有效值为dl＝rms(l)，其中，l表示位置坐标误差，dl表示位置坐标误差的有效值。
[0161]
参照图8，第一方面，本技术实施例提供了一种根据雅克比矩阵和位置坐标误差得到目标校准参数的方法，包括但不限于步骤s610、步骤s620。
[0162]
步骤s610，根据雅克比矩阵、位置坐标误差和预设的更新系数，计算得到海森矩阵和误差向量；
[0163]
可以理解的是，本技术的关节位姿的校准方法通过雅克比矩阵计算得到了海森矩阵，海森矩阵为：
[0164]
h＝(j
t
·
j μ
·
i)
[0165]
其中，h表示海森矩阵，j表示雅克比矩阵，j
t
表示雅克比矩阵的倒置，μ表示预设的更新系数，i表示一个单位矩阵，其中第一校准参数的数量决定着i的行数和列数。
[0166]
可以理解的是，本技术的关节位姿的校准方法通过雅克比矩阵和位置坐标误差计算得到误差向量，误差向量为：
[0167]
g＝j
t
·
l
[0168]
其中，g表示误差向量，j表示雅克比矩阵，j
t
表示雅克比矩阵的倒置，l表示位置坐标误差。
[0169]
步骤s620，根据海森矩阵和误差向量计算得到目标校准参数。
[0170]
可以理解的是，本技术的关节位姿的校准方法结合了levenberg-marquard算法，持续对目标校准参数迭代更新，而目标校准参数是由海森矩阵和误差向量计算得到，目标校准参数为：
[0171]
η
k 1
＝ηk h-1
·g[0172]
其中，η
k 1
表示目标校准参数，ηk表示第一校准参数，h-1
表示表示海森矩阵的倒置，g表示误差向量。
[0173]
需要说明的是，levenberg-marquard算法的迭代更新便包括不断对海森矩阵和误差向量进行更新，即对目标校准参数进行更新，并将更新的目标校准参数输入第一名义位
姿，具体的是，在第一次的迭代更新过程中ηk表示第一校准参数，在后续的迭代更新过程中ηk表示上一次更新得到的目标校准参数。本技术在采集码垛机器人的关节位姿前便已经预设好初始更新次数和最大更新次数，同时随着levenberg-marquard算法的迭代更新，更新次数也将加一。
[0174]
需要说明的是，本技术所述的更新次数随着levenberg-marquard算法的迭代更新加一只是为了更好地说明本技术的关节位姿的校准方法的校准过程，并不构成对本技术的限定，可以理解的是，更新次数可以随着levenberg-marquard算法的迭代更新加一，也可以为，设置最大更新次数，更新次数从最大更新次数计起，并随着levenberg-marquard算法的迭代更新减一，本技术并不对其做具体的限定，能够完成本技术的关节位姿的校准即可。
[0175]
可以理解的是，更新第一关节空间向量，包括以下之一：
[0176]
当预设的初始更新次数等于第一更新阈值，更新第一关节空间向量；或者
[0177]
当初始更新次数大于第一更新阈值且小于第二更新阈值，且当前位置坐标误差的有效值小于上一位置坐标误差的有效值，对更新系数执行缩小操作后，更新第一关节空间向量；或者，
[0178]
当初始更新次数大于第一更新阈值且小于第二更新阈值，且当前位置坐标误差的有效值大于上一位置坐标误差的有效值，对更新系数执行放大操作后，更新第一关节空间向量。
[0179]
需要说明的是，本技术默认第一更新阈值为1，第二更新阈值为预设的最大更新次数，当预设的更新次数等于第一更新阈值，即第一次迭代更新时，需重新将更新的第一关节空间向量输入到运动学模型函数，以进行第二次迭代更新，并更新目标校准系数。
[0180]
当更新次数大于预设的第一更新阈值且小于第二更新阈值，即当前的更新次数介于初始更新次数和最大更新次数之间，且当前计算得到的位置坐标误差的有效值小于上一迭代更新计算得到的位置坐标误差的有效值，需要将更新系数除以十，再改变上次迭代更新后计算得到的目标校准参数的数值，使其等于本次迭代更新后计算得到的目标校准参数的数值，以更新得到下一次迭代更新运用的更新系数，再重新将更新的第一关节空间向量输入到运动学模型函数，以进行第二次迭代更新，并更新目标校准系数。
[0181]
当当前的更新次数大于第一更新阈值且小于第二更新阈值，且当前位置坐标误差的有效值大于上一位置坐标误差的有效值，需要将更新系数乘以十，以更新得到下一次迭代更新运用的更新系数，再重新将更新的第一关节空间向量输入到运动学模型函数，以进行第二次迭代更新，并更新目标校准系数。
[0182]
可以理解的是，校准条件包括以下之一：
[0183]
初始更新次数等于第二更新阈值；或者，
[0184]
初始更新次数大于第一更新阈值且小于第二更新阈值，且当前位置坐标误差的有效值对应上一位置坐标误差的有效值的差小于或等于第二向量阈值；或者，
[0185]
初始更新次数大于第一更新阈值且小于第二更新阈值，且误差向量的模小于或等于第一向量阈值。
[0186]
需要说明的是，当初始更新次数等于第二更新阈值，即当前的更新次数为最大更新次数，满足校准条件，退出迭代更新，并确定了当前的目标校准系数为最终得到的目标校准系数，目标校准系数用于校准码垛机器人的关节位姿。
[0187]
当更新次数大于第一更新阈值且小于第二更新阈值，且当前计算得到的位置坐标误差的有效值减去上一次迭代更新得到的位置坐标误差的有效值，得到的位置坐标误差的有效值的差小于预设的第二向量阈值，退出迭代更新，并确定了当前的目标校准系数为最终得到的目标校准系数，目标校准系数用于校准码垛机器人的关节位姿。
[0188]
当更新次数大于第一更新阈值且小于第二更新阈值，且当前计算得到的误差向量的模小于预设的第一向量阈值，退出迭代更新，并确定了当前的目标校准系数为最终得到的目标校准系数，目标校准系数用于校准码垛机器人的关节位姿。
[0189]
需要说明的是，第一向量阈值与第二向量阈值相等，且都等于0.001。
[0190]
第二方面，参照图9，本技术实施例提供了一种关节位姿的校准系统，包括：
[0191]
至少一个存储器900；
[0192]
至少一个处理器800；
[0193]
至少一个程序；
[0194]
程序被存储在存储器900中，处理器800执行至少一个程序以实现：
[0195]
如本技术第一方面任一项实施例的关节位姿的校准方法。
[0196]
处理器800和存储器900可以通过总线或者其他方式连接。
[0197]
存储器900作为一种非暂态可读存储介质，可用于存储非暂态软件指令以及非暂态性可指令。此外，存储器900可以包括高速随机存取存储器900，还可以包括非暂态存储器900，例如至少一个磁盘存储器件900、闪存器件、或其他非暂态固态存储器件900。可以理解的是，存储器900可选包括相对于处理器800远程设置的存储器900，这些远程存储器900可以通过网络连接至该处理器800。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
[0198]
处理器800通过运行存储在存储器900中的非暂态软件指令、指令以及信号，从而各种功能应用以及数据处理，即实现上述第一方面实施例的关节位姿的校准方法。
[0199]
实现上述实施例的关节位姿的校准方法所需的非暂态软件指令以及指令存储在存储器900中，当被处理器800执行时，执行本技术第一方面实施例的关节位姿的校准方法，例如，执行以上描述的图3中的方法步骤s110至s150、图4中的方法步骤s210至s230、图5中的方法步骤s310至s330、图6中的方法步骤s410至s420、图7中的方法步骤s510至s530、图8中的方法步骤s610至s620。
[0200]
第三方面，本技术实施例提供了一种计算机可读存储介质，计算机可读存储介质存储有计算机可执行信号，计算机可执行信号用于执行：
[0201]
如申请第一方面任一项实施例的关节位姿的校准方法。
[0202]
例如执行以上描述的图3中的方法步骤s110至s150、图4中的方法步骤s210至s230、图5中的方法步骤s310至s330、图6中的方法步骤s410至s420、图7中的方法步骤s510至s530、图8中的方法步骤s610至s620。
[0203]
以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
[0204]
通过以上的实施方式的描述，本领域普通技术人员可以理解，上文中所公开方法
中的全部或某些步骤、系统可以被实施为软件、固件、硬件及其适当的组合。某些物理组件或所有物理组件可以被实施为由处理器，如中央处理器、数字信号处理器或微处理器执行的软件，或者被实施为硬件，或者被实施为集成电路，如专用集成电路。这样的软件可以分布在可读介质上，可读介质可以包括计算机存储介质(或非暂时性介质)和通信介质(或暂时性介质)。如本领域普通技术人员公知的，术语计算机存储介质包括在用于存储信息(诸如计算机可读信号、数据结构、指令模块或其他数据)的任何方法或技术中实施的易失性和非易失性、可移除和不可移除介质。计算机存储介质包括但不限于ram、rom、eeprom、闪存或其他存储器技术、cd-rom、数字多功能盘(dvd)或其他光盘存储、磁盒、磁带、磁盘存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问的任何其他的介质。此外，本领域普通技术人员公知的是，通信介质通常包含计算机可读信号、数据结构、指令模块或者诸如载波或其他传输机制之类的调制数据信号中的其他数据，并且可包括任何信息递送介质。
[0205]
上面结合附图对本技术实施例作了详细说明，但是本技术不限于上述实施例，在所属技术领域普通技术人员所具备的知识范围内，还可以在不脱离本技术宗旨的前提下，做出各种变化。

技术特征：
1.一种关节位姿的校准方法，应用于码垛机器人，其特征在于，包括：采集所述码垛机器人的关节位姿，对所述关节位姿进行计算，得到若干个第一关节空间向量和若干个第一测量位姿；根据所述码垛机器人的结构得到运动学模型函数和第一校准参数，将所述第一关节空间向量输入到所述运动学模型函数，得到第一名义位姿；根据所述第一测量位姿和所述第一名义位姿计算得到第二名义位姿，对所述第一名义位姿和所述第二名义位姿进行计算，得到位置坐标误差；根据所述第一校准参数计算得到所述第一校准参数对应的雅克比矩阵，根据所述雅克比矩阵和所述位置坐标误差计算得到目标校准参数；更新所述第一关节空间向量，重新将更新的所述第一关节空间向量输入到所述运动学模型函数，以更新所述目标校准参数，直至满足校准条件，其中，满足所述校准条件的目标校准参数用于校准所述码垛机器人的关节位姿。2.根据权利要求1所述的关节位姿的校准方法，其特征在于，所述根据所述码垛机器人的结构得到运动学模型函数和第一校准参数，包括：根据所述码垛机器人的结构建立连杆坐标系；根据所述连杆坐标系得到所述码垛机器人的连杆对应的dh参数和第一校准参数，其中所述dh参数包括连杆参数、连杆扭角、连杆偏距和连杆关节角，所述第一校准参数包括连杆参数、减速比修正系数、零位修正系数和工具坐标；根据所述dh参数和所述第一校准参数构建运动学模型函数。3.根据权利要求2所述的关节位姿的校准方法，其特征在于，所述根据所述dh参数和所述第一校准参数构建运动学模型函数，包括：根据所述dh参数构建dh连杆转换矩阵；根据所述dh连杆转换矩阵和所述工具坐标，计算得到所述码垛机器人末端的位姿矩阵；从所述位姿矩阵中提取得到运动学模型函数。4.根据权利要求1所述的关节位姿的校准方法，其特征在于，所述根据所述第一测量位姿和所述第一名义位姿计算得到第二名义位姿，包括：根据所述第一测量位姿和所述第一名义位姿，使用svd方法计算得到位姿转换矩阵；对所述位姿转换矩阵和所述第一测量位姿执行相乘操作，得到第二名义位姿。5.根据权利要求4所述的关节位姿的校准方法，其特征在于，所述根据所述第一测量位姿和所述第一名义位姿，使用svd方法计算得到位姿转换矩阵，包括：分别计算所述第一测量位姿构成的第一点集的重心和所述第一名义位姿构成的第二点集的重心；对所述第一点集的重心和所述第二点集的重心执行重合操作，得到所述第一点集的重心相对于所述第二点集的重心的第一相对坐标点集，和所述第二点集的重心相对于所述第一点集的重心的第二相对坐标点集；根据所述第一相对坐标点集和所述第二相对坐标点集构建协矩阵，对所述协矩阵进行svd分解，根据分解后的协矩阵，计算得到位姿转换矩阵。6.根据权利要求1所述的关节位姿的校准方法，其特征在于，所述根据所述雅克比矩阵
和所述位置坐标误差计算得到目标校准参数，包括：根据所述雅克比矩阵、所述位置坐标误差和预设的更新系数，计算得到海森矩阵和误差向量；根据所述海森矩阵和所述误差向量计算得到目标校准参数。7.根据权利要求6所述的关节位姿的校准方法，其特征在于，所述更新所述第一关节空间向量，包括以下之一：当预设的初始更新次数等于第一更新阈值，更新所述第一关节空间向量；或者，当所述初始更新次数大于所述第一更新阈值且小于第二更新阈值，且当前位置坐标误差的有效值小于上一位置坐标误差的有效值，对所述更新系数执行缩小操作后，更新所述第一关节空间向量；或者，当所述初始更新次数大于所述第一更新阈值且小于所述第二更新阈值，且当前位置坐标误差的有效值大于上一位置坐标误差的有效值，对所述更新系数执行放大操作后，更新所述第一关节空间向量。8.根据权利要求7所述的关节位姿的校准方法，其特征在于，所述校准条件包括以下之一：所述初始更新次数等于所述第二更新阈值；或者所述初始更新次数大于所述第一更新阈值且小于所述第二更新阈值，且当前位置坐标误差的有效值对应上一位置坐标误差的有效值的差小于或等于第二向量阈值；或者，所述初始更新次数大于所述第一更新阈值且小于所述第二更新阈值，且所述误差向量的模小于或等于第一向量阈值。9.一种关节位姿的校准系统，其特征在于，包括：至少一个存储器；至少一个处理器；至少一个程序；所述程序被存储在所述存储器中，所述处理器执行至少一个所述程序以实现如权利要求1至权利要求8任一项所述的关节位姿的校准方法。10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行信号，所述计算机可执行信号用于执行如权利要求1至权利要求8任一项所述的关节位姿的校准方法。

技术总结
本申请公开了一种关节位姿的校准方法、系统及存储介质，应用于码垛机器人，包括：采集码垛机器人的关节位姿，对关节位姿进行计算，得到若干个第一关节空间向量和若干个第一测量位姿；根据码垛机器人的结构得到运动学模型函数和第一校准参数，将第一关节空间向量输入到运动学模型函数，得到第一名义位姿；根据第一测量位姿和第一名义位姿计算得到第二名义位姿，对第一名义位姿和第二名义位姿进行计算，得到位置坐标误差。本申请的关节位姿的校准方法能够有效提高码垛机器人的控制精度。法能够有效提高码垛机器人的控制精度。法能够有效提高码垛机器人的控制精度。


技术研发人员：田坤淼 谢双胜 郭鹏 杨思远 李家晖
受保护的技术使用者：伯朗特机器人股份有限公司
技术研发日：2022.03.30
技术公布日：2022/5/25