一种基于三维点云模型的智能消防机器人控制方法与流程

技术特征：

1.基于三维点云模型的智能消防机器人控制方法，其特征在于步骤如下：

步骤1、通过三维激光雷达现场扫描建立现场三维点云模型；

步骤2、建立机器人坐标系和三维点云模型的映射关系；

步骤3、将红外相机捕获到画面的异常升温点与三维点云模型进行比对，提取异常升温点坐标；

步骤4、确定机器人末端期待位姿，建立齐次坐标矩阵；

步骤5、使用pieper方法求解机器人各轴关节角度，选择最小移动角度为最优解；

步骤6、控制机器人移动至相应位置，进行降温，直至温度降至告警阈值以下；

步骤7、结束降温，机器人复位。

2.根据权利要求1所述的基于三维点云模型的智能消防机器人控制方法，其特征在于，步骤2中，机器人上电，将立柱电机降至立柱最底部；通过示教器上的按钮调整各轴，使得机器人第二轴第三轴间的连杆垂直于立柱，第三轴第四轴连杆与立柱平面，第六轴的轴线z方向指向世界坐标系z轴的反方向，将此位置设置为机械臂各轴电机原点；将机械臂调整至最佳启动位姿待机，记录并保存此时机械臂位姿的关节偏移与世界坐标；确定立柱电机底座为三维点云模型的原点，六轴机械臂底座为机器人坐标系原点，建立机器人坐标系与三维点云模型的映射关系。

3.根据权利要求1所述的基于三维点云模型的智能消防机器人控制方法，其特征在于，步骤4具体如下：

计算六轴机器人的正运动学，由于机器人各关节参数已知，可求解其末端坐标系{e}相对于基坐标系{b}的变换映射；

其中，关节坐标系到的变换关系为：

在上式中，i为机器人各轴的编号，其取值范围为1到6；英文字母为连杆长度，表示关节轴i-1和关节轴i之间的公共法线长度；希腊字母为连杆扭转，表示关节轴i相对于关节轴i-1绕它们的公共法线旋转的角度；为连杆偏移，表示关节i与关节i 1的公共法线和它与关节i-1的公共法线沿这个关节轴的距离；为关节转角，表示关节i与关节i 1的公共法线和它与关节i-1的公共法线绕这个关节轴的转角；表示坐标轴旋转角度的姿态描述；表示坐标轴旋转角度的姿态描述；表示沿着坐标轴进行长度的平移变换；表示沿着坐标轴进行长度的平移变换；

其一般表达式为：

上式中，，，，各参数含义均与（1）式相同；将所有六个关节相邻关节之间的变换映射由上式表示，即可获得由末端关节坐标{e}系到基座标系{b}的映射关系:

其中、、、、、分别表示其下表所示坐标系到上标所示坐标系的齐次坐标变换矩阵；

期望末端齐次变换矩阵的确定：在所建世界坐标系与机器人坐标系的基础上，通过红外相机获得机器人可达空间外异常升温点与基坐标系原点连线交机器人可达空间球面于一点，交点即为期望末端位置坐标点，作为平移向量计算的依据，取该交点处球面的法线作为末端坐标系z轴的正方向，对末端坐标系x轴y轴进行与末端z轴相同的旋转变换可得期望旋转矩阵，至此，完成对机器人末端期望齐次变换矩阵的确定；

1)直线方程的确定：

首先是确定空间直线的方程，取机器人可达空间外一点和机器人基坐标系原点构造空间直线方程如下：

其中，为参变量，为基坐标系原点，空间直线的方向向量可由机器人可达空间外一点即异常升温点坐标和机器人基坐标系原点共同确定：

故最终确定的直线方程为：

而机器人可达空间的球面方程为：

其中r为六轴机器人可达空间最外层球壳的半径；

2)联立求交点：将上述直线方程同球面方程联立，可得关于参变量t的一元二次方程：

解出t=±r，带入(6)式，即可求得由异常升温点和机器人基座标系原点确定的空间直线与机器人可达空间球面方程的交点坐标在机器人基座标系中的表示；

末端位姿的确定：由式(5)，可得机器人末端z轴方向向量，分别求其关于基座标系平面的投影及平面的投影，以同样的方法求得机器人初始位姿下末端z轴分量在基坐标系下平面的投影及平面的投影，即可求得由初始位姿z轴到期望位姿z轴的旋转矩阵，令初始位姿机器人末端x轴、y轴方向向量进行相同的旋转变换，即可获得期望的末端x轴、y轴方向向量，完成对末端期望齐次变换矩阵的确定。

4.根据权利要求3所述的基于三维点云模型的智能消防机器人控制方法，其特征在于，步骤5具体如下：

当机器人的4、5、6轴轴线相交于一点时，最后三轴的原点均位于此交点上，该交点在基座标系中的位置坐标为：

式中，为包含4、5、6轴坐标系原点在基座标系中的位置信息的矩阵，x、y、z表示4、5、6轴坐标系原点在基座标系中的三维坐标；为包含4、5、6轴坐标系原点在第三轴坐标系中的位置信息的矩阵；、、分别表示其下标所示关节轴坐标系到其上标所示关节轴坐标系的齐次坐标变换矩阵，具体计算方法见式（2）；

当时，可由（2）式中的变换第四列求得最后三轴的交点坐标在基坐标系中的表示：

式中、、均表示其下标对应关节轴相关的dh参数，，；为了便于表示和计算，对上式进行数学代换：

其中、、为以第三轴关节转角为自变量的函数表达式，其具体含义如下：

根据式（2），得到上式中、、的表达式：

式中，、、、、、、分别表示其下标对应关节轴的dh参数，表示，表示，表示，表示，分别表示与；同样对于,应用式（2）可得：

式中、分别表示与，为第一关节轴转角，

其中，、、计算方式见式（13），、分别表示与，为第二关节轴转角；其余参数与式（13）中相应参数含义相同；计算的坐标平方和：

由（8）可得：

进行变量替换消去因变量，化简的关系式，同时写出z方向分量的方程：

其中,

由式(18)求解第三关节轴关节转角，在此处分三种情况：

若=0，则r=，此处r已知而等式右侧的仅为关于的函数，用三角函数万能公式进行变量替换后可求解；

若=0，则，此处已知，再次使用三角函数万能公式，利用变量替换即可求解；

若上述两种情况都不满足，则利用方程组消去和，得到：

代入三角函数万能公式，可得到一元四次方程，由此可解算出；

在成功解算出后，可由上述方程求解第二关节轴和第一关节轴转角和；

、、的求解：

在解出前三关节轴转角、、后，由于第四、第五、第六轴的轴线相交于一点，故这些关节角只影响末端连杆的方向；此处先令第四关节轴转角，计算出此时的连杆坐标系{4}相对于基坐标系的旋转矩阵，坐标系{6}的期望方向与连杆坐标系{4}的区别在于后三个关节的作用，由于已知，故可解出：

对于绝大多数机械臂，可由z-y-z欧拉角解法应用于时的解出第四、第五、第六轴的角度值；

在所有解中寻找机器人各关节移动距离最短的解作为最优解。

5.根据权利要求1所述的基于三维点云模型的智能消防机器人控制方法，其特征在于，步骤6中，工控机将解算出的关节角度值发送给智能消防机器人，等待机器人运动至相应的异常温升点；机器人运动结束后，打开高压细水雾分区控制阀，对异常升温点进行喷洒降温，直至异常升温点温度降至告警阈值以下。

技术总结
本发明公开一种基于三维点云模型的智能消防机器人控制方法，包括以下步骤：建立机器人坐标系和三维点云模型映射关系，从红外相机捕获到的画面中提取异常升温点坐标后，确定机器人末端的期待位姿，建立末端期待齐次变换矩阵，然后使用Pieper方法求解机器人各轴关节角度，选择最近解为最优解，控制机器人移动至期望位置开启高压细水雾喷枪阀门进行降温，直至温度降至告警阈值以下，结束降温，机器人复位。本发明使用旋转平移矩阵和三轴相交的Pieper方法，借助现场三维点云模型，能够实现六轴消防机器人对着火场异常升温点的精确定位，能够避免因变压器异常升温甚至发生火灾爆炸对工作人员人身安全、电力系统带来的威胁和损失。

技术研发人员：汤锦慧;伍发元;肖齐;辛建波;刘专;毛梦婷
受保护的技术使用者：国网江西省电力有限公司电力科学研究院;国网江西省电力有限公司;国家电网有限公司
技术研发日：2021.03.17
技术公布日：2021.07.13