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

本发明涉及变电站消防领域，具体涉及一种基于三维点云模型的智能消防机器人控制方法。
背景技术：
：目前现有的变压器水介质的消防系统多采用低压框架式水喷淋灭火系统，大多采用固定式喷头，安装复杂，占地面积大，喷头数量多，运维工作量大，协同效果差，喷水效果不佳，灭火效果不理想；并且各消防子系统都是独立的消防系统，存在集中管理程度低、无本地综合预警和远程交互功能、联动及时性差、信息系统不具备智能分析能力等问题，不符合国家电网“变电站细水雾灭火技术”要求，因此急需一种安全、经济、有效的消防灭火手段。同现有手段相比，应用了六轴机器人的智能消防系统具有设备结构小、施工成本低、灭火启动快、便于控制、及时消灭早期火灾、灭火由人工智能控制或人员远程参与控制、灭火人员远离火灾现场、安全性高，高压细水雾节约用水、灭火效果好，现场设备简单不影响主变日常维护检修等优点。六自由度的机器人运动学反解非常复杂，一般没有封闭解。在应用d-h法建立运动学方程的基础上，进行一定的解析计算后发现，位置反解往往有很多个，不能得到有效的封闭解。技术实现要素：针对目前技术存在的不足，本发明的目的在于提供一种基于三维点云模型的智能消防机器人控制方法，此种控制方法将红外相机识别到的异常升温点的位置信息与三维点云模型进行比对，得到异常升温点的坐标位置，通过旋转平移矩阵和pieper方法，自动解算出六轴消防机器人各关节角转动的角度，为六轴消防机器人的定位提供辅助。应用此种控制方法的智能消防机器人，配合立柱电机及高压细水雾泵组，能够实现对异常升温点的精准、快速降温，大大提高了变电站现场消防系统的安全性。为了达到上述目的，本发明采用如下技术方案：基于三维点云模型的智能消防机器人控制方法，具体步骤如下：步骤1、通过三维激光雷达现场扫描建立现场三维点云模型；步骤2、建立机器人坐标系和三维点云模型的映射关系；步骤3、将红外相机捕获到画面的异常升温点与三维点云模型进行比对，提取异常升温点坐标；步骤4、确定机器人末端期待位姿，建立齐次坐标矩阵；步骤5、使用pieper方法求解机器人各轴关节角度，选择最小移动角度为最优解；步骤6、控制机器人移动至相应位置，进行降温，直至温度降至告警阈值以下；步骤7、结束降温，机器人复位。具体地，步骤2中，机器人上电，将立柱电机降至立柱最底部；通过机器人示教器上的按钮手动调整各轴，使得机器人第二轴第三轴间的连杆垂直于立柱，第三轴第四轴连杆与立柱平行，第六轴的轴线z方向指向世界坐标系z轴的反方向，将此位置设置为机械臂各轴电机原点；将机械臂调整至最佳启动位姿待机，记录并保存此时机械臂位姿的关节偏移与世界坐标；确定立柱电机底座为三维点云模型的原点，六轴机械臂底座为机器人坐标系原点，建立机器人坐标系与三维点云模型的映射关系。具体地，步骤4中，计算机器人的正运动学，由于机器人各关节参数已知，可求解其末端坐标系{e}相对于基坐标系{b}的变换映射。其中，关节坐标系i到i-1的变换关系为：在上式中，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轴方向向量，完成对末端期望齐次变换矩阵的确定。具体地，步骤5中，在已知机器人期望末端齐次变换矩阵的基础上，可以对机器人进行逆运动学求解，但是六自由度的机器人的运动学反解非常复杂，一般没有封闭解。在应用d-h法建立运动学方程的基础上，使用三轴相交的pieper解法进行机器人运动学逆运算的求解：当机器人的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)求解第三关节轴关节转角，在此处分三种情况：1)若=0，则r=，此处r已知而等式右侧的仅为关于的函数，用三角函数万能公式进行变量替换后可求解。2)若=0，则，此处已知，再次使用三角函数万能公式，利用变量替换即可求解.3)若上述两种情况都不满足，则利用方程组消去和，得到：带入三角函数万能公式，可得到一元四次方程，由此可解算出;在成功解算出后，可由上述方程求解第二关节轴和第一关节轴转角和;在解出前三关节轴转角、、后，由于第四、第五、第六轴的轴线相交于一点，故这些关节角只影响末端连杆的方向。此处先令第四关节轴转角，计算出此时的连杆坐标系{4}相对于基坐标系的旋转矩阵，坐标系{6}的期望方向与连杆坐标系{4}的区别在于后三个关节的作用，由于已知，故可解出连杆坐标系{4}相对于机器人末端坐标系的旋转矩阵：对于绝大多数机械臂，可由欧拉角解法应用于第四关节轴时的解出第四、第五、第六轴的角度值。在所有解中寻找机器人各关节移动距离最短的解作为最优解。具体地，步骤6中：工控机将解算出的关节角度值发送给智能消防机器人，等待机器人运动至相应的异常温升点；机器人运动结束后，打开高压细水雾分区控制阀，对异常升温点进行喷洒降温，直至异常升温点温度降至告警阈值以下。本发明应用pieper方法，借助三维点云模型点云数据事先确定异常升温点的位置和机械臂运动至期望点的位姿，由异常升温点位置求取机械臂前三轴的关节转角，然后由机器人期望位姿计算得出机器人后三轴的关节转角，实现了对于六关节均为旋转关节且最后三轴相交的6自由度机器人封闭解的求取，相对于其他逆运动学求解方法，具有运算过程简单，运算速度快等优点。同时，将三维点云模型和红外相机监控画面相结合，可快速得到异常升温点的位置坐标，能够提高整个智能消防系统的响应速度。本发明所述的基于三维点云模型的智能消防机器人控制方法可应用在一般工控机上，配备红外摄像头辅助定位，可实现对异常升温点的精准、快速降温，大大提高了变电站现场消防系统的安全性。附图说明图1为本发明控制方法原理图。具体实施方式：下面结合智能消防机器人搭载的红外摄像头、滑杆电机、泵组分区控制阀做更详细的说明。参照图1，本发明的具体过程如下：步骤1：通过三维激光雷达现场扫描建立现场三维点云模型：三维激光雷达扫描得到变电站现场的激光点云数据，建立现场三维点云模型。步骤2：建立机器人坐标系和三维点云模型的映射关系：机器人上电，将立柱电机降至立柱最底部；通过机器人示教器上的按钮手动调整各轴，使得机器人第二轴第三轴间的连杆垂直于立柱，第三轴第四轴连杆与立柱平行，第六轴的轴线z方向指向世界坐标系z轴的反方向，将此位置设置为机械臂各轴电机原点。将机械臂调整至最佳启动位姿待机，记录并保存此时机械臂位姿的关节偏移与世界坐标。确定立柱电机底座为三维点云模型的原点，六轴机械臂底座为机器人坐标系原点，建立机器人坐标系与三维点云模型的映射关系。步骤3：将红外相机捕获到画面的异常升温点与三维点云模型进行比对，提取异常升温点坐标；步骤4：确定机器人末端期待位姿，建立齐次坐标矩阵：计算六轴机器人的正运动学，由于机器人各关节参数已知，可求解其末端坐标系{e}相对于基坐标系{b}的变换映射，本发明所应用六轴机器人d-h参数下表所示。关节杆长扭角杆距d(mm)转角100027090°0339000412590°39450-90°06090°119其中，关节坐标系i到i-1的变换关系为：在上式中，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为六轴机器人可达空间最外层球壳的半径。本发明所用六轴机器人r=1800mm。2)联立求交点：将上述直线方程同球面方程联立，可得关于参变量t的一元二次方程。解出t=±r，带入(6)式，即可求得由异常升温点和机器人基座标系原点确定的空间直线与机器人可达空间球面方程的交点坐标在机器人基座标系中的表示。末端位姿的确定：由式(5)，可得机器人末端z轴方向向量，分别求其关于基座标系平面的投影及平面的投影，以同样的方法求得机器人初始位姿下末端z轴分量在基坐标系下平面的投影及平面的投影，即可求得由初始位姿z轴到期望位姿z轴的旋转矩阵，令初始位姿机器人末端x轴、y轴方向向量进行相同的旋转变换，即可获得期望的末端x轴、y轴方向向量，完成对末端期望齐次变换矩阵的确定。步骤5：使用pieper方法求解机器人各轴关节角度，选择最小移动角度为最优解：在已知机器人期望末端齐次变换矩阵的基础上，可以对机器人进行逆运动学求解，但是六自由度的机器人的运动学反解非常复杂，一般没有封闭解。在应用d-h法建立运动学方程的基础上，应用三轴相交的pieper解法进行机器人运动学逆运算的求解：当机器人的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方向分量的方程：其中,由上式求解，在此处分三种情况：1)若=0，则r=，此处r已知而等式右侧的仅为关于的函数，用三角函数万能公式进行变量替换后可求解。2)若=0，则，此处已知，再次使用三角函数万能公式，利用变量替换即可求解.3)若上述两种情况都不满足，则利用方程组消去和，得到：带入三角函数万能公式，可得到一元四次方程，由此可解算出.在成功解算出后，可由上述方程求解和.、、的求解：在解出前三关节轴转角、、后，由于第四、第五、第六轴的轴线相交于一点，故这些关节角只影响末端连杆的方向。此处先令第四关节轴转角，计算出此时的连杆坐标系{4}相对于基坐标系的旋转矩阵，坐标系{6}的期望方向与连杆坐标系{4}的区别在于后三个关节的作用，由于已知，故可解出连杆坐标系{4}相对于机器人末端坐标系的旋转矩阵：对于绝大多数机械臂，可由z-y-z欧拉角解法应用于时的解出第四、第五、第六轴的角度值。在所有解中寻找机器人各关节移动距离最短的解作为最优解。步骤6：控制机器人移动至相应位置，进行降温，直至温度降至告警阈值以下：工控机将解算出的关节角度值发送给智能消防机器人，等待机器人运动至相应的异常温升点。机器人运动结束后，打开高压细水雾分区控制阀，对异常升温点进行喷洒降温，直至异常升温点温度降至告警阈值以下。步骤7：完成降温过程，机器人复位，等待下一次启动。将本发明所述的智能消防机器人控制方法应用在一般工控机上，配备摄像头辅助定位，可实现对异常升温点的精准、快速降温，大大提高了变电站现场消防系统的安全性。当前第1页12