一种基于ICP匹配算法的角钢塔三维空间模型验证方法

一种基于icp匹配算法的角钢塔三维空间模型验证方法
技术领域
1.本发明涉及三维建模技术领域，尤其是一种基于icp匹配算法的角钢塔三维空间模型验证方法。


背景技术：

2.随着电网新基建智能化设备进程不断深入，电力攀爬机器人的研制成为一大热点。目前，输电线路角钢塔的维修检测方式仍然是人工检测，这种方法费时费力，甚至威胁到电力工人的生命安全。国内现有的针对输电线路角钢塔攀爬机器人的研究成果较少，且存在角钢塔放样软件建立的信息化模型与传统的机器人仿真平台模型无法兼容一大研究难点。如何对复杂的角钢塔进行可视化建模，生成标准的角钢塔信息化模型数据格式，并验证角钢塔实体数据与在机器人仿真软件中模型数据的精确度，满足要求后从而利用机器人进行仿真任务作业，这已经成为近年来所亟待解决的技术难题。


技术实现要素：

3.为了克服上述现有技术中的缺陷，本发明提供一种基于icp匹配算法的角钢塔三维空间模型验证方法，对复杂的角钢塔进行可视化建模，生成能够在机器人仿真软件中进行完全加载的数据格式，并验证角钢塔的实体数据与在机器人仿真软件中模型数据的匹配度，判断角钢塔仿真三维模型是否满足机器人仿真实验的要求。
4.为实现上述目的，本发明采用以下技术方案，包括：
5.一种基于icp匹配算法的角钢塔三维空间模型验证方法，
6.s1，根据角钢塔的实体数据，利用三维软件对角钢塔进行三维建模，生成角钢塔初始三维模型；
7.角钢塔的实体数据包括：角钢塔中各个零部件在世界坐标系中的位置坐标，以及各个零部件之间的连接关系；
8.s2，从角钢塔初始三维模型中提取出角钢塔的空间结构以及角钢塔的零部件数据，角钢塔的零部件数据包括：零部件的种类、编号id、在三维软件坐标系中的位置坐标；
9.s3，将从角钢塔初始三维模型中提取的零部件数据转换为标准数据格式，用于在机器人仿真软件中运行；
10.s4，将标准数据格式的零部件数据导入机器人仿真软件，生成角钢塔仿真三维模型；
11.s5，运用icp匹配算法计算角钢塔的实体数据与角钢塔仿真三维模型中的模型数据之间的匹配度，若匹配度满足要求，则表示角钢塔仿真三维模型满足机器人仿真实验的要求；若匹配度不满足要求，则表示角钢塔仿真三维模型不满足机器人仿真实验的要求。
12.优选的，步骤s5中，运用icp匹配算法计算角钢塔的实体数据与仿真三维模型中的模型数据之间的匹配度，具体如下所示：
13.s51，从角钢塔实体上选取n个实体零部件，利用角钢塔实体上的此n个实体零部件
在世界坐标系中的位置坐标，构建标准点集q：
14.q＝{q1,q2,...,qn}＝{qi|i＝1,2,
…
n}
15.其中，qi表示标准点集q中的第i个数据，qi为角钢塔实体上所选取的第i个实体零部件在世界坐标系中的位置坐标；
16.s52，从角钢塔仿真三维模型中选取与角钢塔实体上所选取的n个实体零部件一一对应的n个模型零部件，利用角钢塔仿真三维模型中的此n个模型零部件在仿真三维模型坐标系中的位置坐标，构建待匹配点集w：
17.w＝{w1,w2,...,wn}＝{wi|i＝1,2,
…
n}
18.其中，wi表示待匹配点集w中的第i个数据，wi为角钢塔仿真三维模型上所选取的第i个模型零部件在仿真三维模型坐标系中的位置坐标；
19.s53，设计目标函数为标准点集和待匹配点集之间的误差函数，目标函数如下所示：
[0020][0021]
其中，r表示旋转矩阵，r为3*3的矩阵，t表示平移向量，t为3*1的向量；
[0022]
s54，根据标准点集和待匹配点集中的数据，计算旋转矩阵r和平移矩阵t，使得目标函数的值最小，具体如下所示：
[0023]
s541，分别计算标准点集和待匹配点集的质心：
[0024][0025][0026]
其中，uq为标准点集的质心，uw为待匹配点集的质心；
[0027]
s542，分别对标准点集和待匹配点集去中心化，得到去中心化后的标准点集和待匹配点集：
[0028]
qi'＝q
i-uq[0029]
wi'＝w
i-uw[0030]
其中，qi'为去中心化后的标准点集中的第i个数据，wi'为去中心化后的待匹配点集中的第i个数据；
[0031]
s543，构建矩阵h：
[0032][0033]
s544，对矩阵h进行svd分解，根据矩阵h的svd分解结果，得到旋转矩阵r和平移矩阵t：
[0034]
h＝u∑v
t
[0035]
r＝vu
t
[0036]
t＝u
q-ruw[0037]
其中，u、v为矩阵h进行svd分解后得到的矩阵；
[0038]
s55，根据步骤s544得到的旋转矩阵r和平移矩阵t，根据目标函数e(r,t)计算标准点集和待匹配点集之间的误差值即匹配度，判断匹配度是否满足阈值要求；
[0039]
若满足，则表示角钢塔的仿真三维模型满足要求；
[0040]
若不满足，则表示角钢塔的仿真三维模型不满足要求，进行下一次迭代，迭代方式为：
[0041]
利用步骤s544得到的旋转矩阵r和平移矩阵t对待匹配点集进行旋转和平移变换，得到新点集，按照步骤s54的方式利用标准点集和新点集中的数据重新计算旋转矩阵r和平移矩阵t，得到新的旋转矩阵r和新的平移矩阵t，并利用新的旋转矩阵r和新的平移矩阵t代入目标函数e(r,t)重新计算标准点集和待匹配点集之间的匹配度，若匹配度满足阈值要求，则表示角钢塔仿真三维模型满足要求，且得到旋转矩阵r和平移矩阵t的最优解，停止迭代；否则，表示角钢塔仿真三维模型仍不满足要求，继续进行下一次迭代，直至超过最大迭代次数退出循环。
[0042]
优选的，步骤s1中，根据角钢塔的实体数据，利用3dsmax三维软件对角钢塔进行三维建模。
[0043]
优选的，步骤s3中，将从角钢塔初始三维模型中提取的零部件数据转换为xml格式。
[0044]
优选的，角钢塔的空间结构是由若干个基础模块构成的，每个基础模块是由若干个零部件连接构成的；
[0045]
步骤s3中，将同一种类的零部件数据以链表的形式进行存储，即每一种类的零部件数据对应有一个链表数据，将各个种类的链表数据存储在一个数组中，最终生成标准数据格式的dae；
[0046]
链表中的零部件数据包含零部件的编号id、在三维软件坐标系中的位置坐标。
[0047]
优选的，步骤s4中，将标准数据格式的零部件数据导入机器人仿真软件时，需要进行模型数据转换，具体如下所示：
[0048]
s41，设角钢塔上某零部件中心点为点p，三维软件坐标系以z轴向上，在角钢塔的初始三维模型中，向量的坐标p
tower
为(x,y,z)，且点p在xy平面、xz平面、yz平面的投影分别为点m、点p、点n；其中，点n的坐标为(z,y)；o为三维软件的坐标系的原点；
[0049]
s42，机器人仿真软件坐标系以y轴向上，在角钢塔的仿真三维模型中，向量绕x轴逆旋转θ角，θ＝90
°
，向量旋转后的坐标p
unity
为(x',y',z')；p
unity
的计算方式如下所示：
[0050]
p
unityt
＝r
x
(θ)p
towert
；
[0051]
其中，r
x
(θ)为绕x轴旋转θ角的旋转矩阵；
[0052][0053]
本发明的优点在于：
[0054]
(1)本发明针对角钢塔信息化建模与机器人模型行业间的壁垒造成现有的三维模型无法兼容的问题，设计一种基于icp匹配算法的角钢塔三维空间模型验证方法，并提出运用icp匹配算法验证角钢塔原始模型数据与在机器人仿真软件中模型数据之间的匹配精确度，满足精度要求后可在角钢塔模型上进行机器人仿真实验。
[0055]
(2)本发明可以将复杂的角钢塔以三维的方式全方位展示给目标人群，生成标准的角钢塔信息化模型数据格式，支持导入机器人领域的仿真软件，直接获取角钢塔关键参数属性信息，在主流的机器人仿真平台如ros系统以及三维游戏引擎unity3d下，形成1:1的真实映射，进而开展攀爬机器人在角钢塔上仿真任务作业。
附图说明
[0056]
图1为本发明的一种基于icp匹配算法的角钢塔三维空间模型验证方法的流程图。
具体实施方式
[0057]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0058]
本发明的一种基于icp匹配算法的角钢塔三维空间模型验证方法，包括以下步骤：
[0059]
s1，针对角钢塔实体进行数据化标注，结合角钢塔建造时cad软件的数据信息，得到角钢塔的实体数据，并生成角钢塔实体数据库；角钢塔的实体数据包括：角钢塔中各个零部件在世界坐标系中的位置坐标和姿态坐标，以及各个零部件之间的连接关系；
[0060]
根据角钢塔的实体数据，在3dsmax三维软件中进行放样生成角钢塔的初始三维模形；
[0061]
s2，从角钢塔初始三维模型中提取出角钢塔的空间结构即csg结构以及角钢塔的零部件数据；
[0062]
角钢塔的零部件数据包括：零部件的种类、编号id、在三维软件坐标系中的位置坐标和姿态坐标；
[0063]
s3，将从角钢塔初始三维模型中所提取的csg结构和零部件数据进行轻量化处理，并输出标准数据格式的dae，用于在机器人仿真软件unity3d中加载；
[0064]
s4，将标准数据格式的dae导入机器人仿真软件unity3d，生成角钢塔的仿真三维模型；
[0065]
其中，角钢塔的零部件之间具备层级关系，层级是指：角钢塔包括角钢塔峰值，角钢塔横臂，角钢塔塔笼，角钢塔塔体四个部分即四个层级，每个层级下又包括主材、斜材、辅材、螺栓、连接板等零部件，在机器人仿真软件unity3d中，角钢塔的零部件之间层级关系会丢失，因此，在机器人仿真软件unity3d中，将角钢塔的零部件之间的层级关系进行父子层
级的重新绑定。
[0066]
s5，运用icp匹配算法计算角钢塔的实体数据与角钢塔仿真三维模型中的模型数据之间的匹配度，若匹配度满足要求，则表示角钢塔仿真三维模型满足机器人仿真实验的要求。
[0067]
步骤s1的具体过程如下所示：
[0068]
s11，确定角钢塔的各个层级中的各个零部件的种类、尺寸，以及零部件之间的连接关系；其中，将零部件的连接关系作为零部件的型号，例如，连接板的型号为：连接主材与斜材，或连接主材与辅材等；
[0069]
s12，根据各个零部件的种类、尺寸，利用3dsmax三维软件分别对各零部件进行建模；根据零部件之间的连接关系，在利用3dsmax三维软件中进行组装，生成角钢塔的初始三维模型。
[0070]
步骤s2中，从角钢塔的初始三维模形中提取出有效数据，有效数据主要为角钢塔的csg结构；
[0071]
csg结构是指通过基本结构模块及施加于其上的几何变换集合运算、局部修改等方法可以构造所需要的输电铁塔的空间结构。由于角钢塔结构主要由角钢通过螺栓连接构成，其规则性相对较强，因而可以利用建立好的初始三维模型提取出csg结构的基本特征，再将基本特征进行模块化分解，将整个角钢塔分解为各种基本结构模块,并逐一生成各基本结构模块的生成模板，确定各基本结构模块中节点和零部件的布置形式和设计规则，然后通过对上述基本结构模块进行空间变换和集合运算，最终生成角钢塔的csg结构。
[0072]
步骤s3中，轻量化处理的方式为：
[0073]
s31，根据步骤s2提取的csg结构以及角钢塔的零部件数据；其中，角钢塔的空间结构是由若干个基础模块构成的，每个基础模块是由若干个零部件连接构成的；
[0074]
将同一种类的零部件数据以链表的形式进行存储，即每一种类的零部件数据对应有一个链表数据，将各个种类的链表数据存储在个数组中。
[0075]
以数组的方式存储，以链表的形式进行增添，方便后期零件的修改：
[0076]
其中，链表中每个相同零件都包含编号id，位置坐标，姿态坐标等，各层次结构以哈希表结构去存储，这样既方便去增添，也便于查找，过滤掉重复数据，提高存储效率。
[0077]
s32，对角钢塔进行哈希结构组装，生成xml底层文件，便于传输。最终生成标准的dae文件，可以快速导入至unity3d机器人仿真平台。
[0078]
步骤s4中，将标准的数据格式dae导入机器人仿真软件时，需要进行模型数据转换，具体如下所示：
[0079]
s41，设角钢塔上某零部件中心点为点p，三维软件的坐标系以z轴向上，在角钢塔的初始三维模型中，向量的坐标p
tower
为(x,y,z)，且点p在xy平面、xz平面、yz平面的投影分别为点m、点p、点n；其中，点n的坐标为(z,y)；
[0080]
o为三维软件的坐标系的原点；
[0081]
s42，机器人仿真软件的坐标系以y轴向上，在角钢塔的仿真三维模型中，向量绕x轴逆旋转θ角，θ＝90
°
，向量旋转后的坐标p
unity
为(x',y',z')；p
unity
的计算方式如下所示：
[0082]
p
unityt
＝r
x
(θ)p
towert
；
[0083]
其中，r
x
(θ)为绕x轴旋转θ角的旋转矩阵；
[0084][0085]
步骤s5中，运用icp匹配算法计算角钢塔的实体数据与仿真三维模型中的模型数据之间的匹配度，具体如下所示：
[0086]
s51，从角钢塔实体上选取n个实体零部件，利用角钢塔实体上的此n个实体零部件在世界坐标系中的位置坐标，构建标准点集q：
[0087]
q＝{q1,q2,...,qn}＝{qi|i＝1,2,
…
n}
[0088]
其中，qi表示标准点集q中的第i个数据，qi为角钢塔实体上所选取的第i个实体零部件在世界坐标系中的位置坐标；
[0089]
s52，从角钢塔仿真三维模型中选取与角钢塔实体上所选取的n个实体零部件一一对应de n个模型零部件，利用角钢塔仿真三维模型中的此n个模型零部件在仿真三维模型坐标系中的位置坐标，构建待匹配点集w：
[0090]
w＝{w1,w2,...,wn}＝{wi|i＝1,2,
…
n}
[0091]
其中，wi表示待匹配点集w中的第i个数据，wi为角钢塔仿真三维模型上所选取的第i个模型零部件在仿真三维模型坐标系中的位置坐标；
[0092]
s53，设计目标函数为标准点集和待匹配点集之间的误差函数，目标函数如下所示：
[0093][0094]
其中，r表示旋转矩阵，r为3*3的矩阵，t表示平移向量，t为3*1的向量；
[0095]
s54，根据标准点集和待匹配点集中的数据，计算旋转矩阵r和平移矩阵t，使得目标函数的值最小，具体如下所示：
[0096]
s541，分别计算标准点集和待匹配点集的质心：
[0097][0098][0099]
其中，uq为标准点集的质心，uw为待匹配点集的质心；
[0100]
s542，分别对标准点集和待匹配点集去中心化，得到去中心化后的标准点集和待匹配点集：
[0101]
qi′
＝q
i-uq[0102]
wi′
＝w
i-uw[0103]
其中，qi′
为去中心化后的标准点集中的第i个数据，wi′
为去中心化后的待匹配点
集中的第i个数据；
[0104]
s543，构建矩阵h：
[0105][0106]
s544，对矩阵h进行svd分解，根据矩阵h的svd分解结果，得到旋转矩阵r和平移矩阵t：
[0107]
h＝u∑v
t
[0108]
r＝vu
t
[0109]
t＝u
q-ruw[0110]
其中，u、v为矩阵h进行svd分解后得到的矩阵；
[0111]
s55，根据步骤s54得到的旋转矩阵r和平移矩阵t，根据目标函数e(r,t)计算标准点集和待匹配点集之间的误差值即匹配度，判断匹配度是否满足阈值要求；
[0112]
若满足，则表示角钢塔的仿真三维模型满足要求，可在机器人仿真软件下进行角钢塔的作业任务；
[0113]
若不满足，则表示角钢塔的仿真三维模型不满足要求，进行下一次迭代，迭代方式为：
[0114]
利用步骤s54得到的旋转矩阵r和平移矩阵t对待匹配点集进行旋转和平移变换，得到新点集，按照步骤s54的方式利用标准点集和新点集中的数据重新计算旋转矩阵r和平移矩阵t，得到新的旋转矩阵r和新的平移矩阵t，并利用新的旋转矩阵r和新的平移矩阵t代入目标函数e(r,t)重新计算标准点集和待匹配点集之间的匹配度，若匹配度满足阈值要求，则表示角钢塔仿真三维模型满足要求，且得到旋转矩阵r和平移矩阵t的最优解，停止迭代；否则，表示角钢塔仿真三维模型仍不满足要求，继续进行下一次迭代，直至超过最大迭代次数退出循环。
[0115]
其中，利用旋转矩阵r和平移矩阵t进行旋转和平移变换为现有技术。
[0116]
本发明中，关于匹配度的阈值要求，根据角钢塔的特征，主要由角钢塔主材，斜材与辅材等角钢零部件构成框架，基于icp算法进行数据匹配时，主材，辅材以及斜材等关键部位的数据点，达到90％的零部件匹配精确度，误差范围在0.1cm即可判断本次角钢塔模型数据匹配验证成功，退出循环。而对于主材，斜材，辅材上的连接板以及覆盖螺栓，连接螺栓，脚钉等非关键特征点，因其不能完全具备特征点，对包含这些非关键特征点的角钢塔数据匹配时，需要匹配的精度阈值需要达到95％的零部件匹配精确度，并且需要结合角钢塔内主材，斜材等关键特征点匹配度，综合验证是否成功。
[0117]
本发明最终是需要建立能够在机器人仿真软件unity3d中进行仿真的角钢塔仿真三维模型。在机器人仿真软件unity3d中建立角钢塔仿真三维模型可以利用unity3d平台自带的建模工具软件实现模型创建，但此种方法对角钢塔上万个零件过于繁琐，实现难度大，也可以使用三维扫描仪器对物理对象进行扫描建模，但是这种方法成本建立的模型误差较大。本发明使用专业的建模软件，通过提供的角钢塔数据库中的零件数据信息创建放样三维模型，由于专业的建模软件3dsmax生成的模型并不能生成直接导入仿真软件unity3d中，所以先生成标准化文件的之后再导入仿真软件unity3d，另外本发明方法建立的角钢塔仿
真三维模型方便后期修改。
[0118]
以上仅为本发明创造的较佳实施例而已，并不用以限制本发明创造，凡在本发明创造的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明创造的保护范围之内。