CN113101584A - 一种基于三维点云模型的智能消防机器人控制方法 - Google Patents

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

Info

Publication number
CN113101584A
CN113101584A CN202110285775.8A CN202110285775A CN113101584A CN 113101584 A CN113101584 A CN 113101584A CN 202110285775 A CN202110285775 A CN 202110285775A CN 113101584 A CN113101584 A CN 113101584A
Authority
CN
China
Prior art keywords
robot
axis
coordinate system
joint
formula
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.)
Pending
Application number
CN202110285775.8A
Other languages
English (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.)
State Grid Jiangxi Electric Power Co ltd
State Grid Corp of China SGCC
Electric Power Research Institute of State Grid Jiangxi Electric Power Co Ltd
Original Assignee
State Grid Jiangxi Electric Power Co ltd
State Grid Corp of China SGCC
Electric Power Research Institute of State Grid Jiangxi Electric Power Co Ltd
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 State Grid Jiangxi Electric Power Co ltd, State Grid Corp of China SGCC, Electric Power Research Institute of State Grid Jiangxi Electric Power Co Ltd filed Critical State Grid Jiangxi Electric Power Co ltd
Priority to CN202110285775.8A priority Critical patent/CN113101584A/zh
Publication of CN113101584A publication Critical patent/CN113101584A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A62—LIFE-SAVING; FIRE-FIGHTING
    • A62C—FIRE-FIGHTING
    • A62C37/00—Control of fire-fighting equipment
    • A—HUMAN NECESSITIES
    • A62—LIFE-SAVING; FIRE-FIGHTING
    • A62C—FIRE-FIGHTING
    • A62C27/00—Fire-fighting land vehicles

Landscapes

  • Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Business, Economics & Management (AREA)
  • Emergency Management (AREA)
  • Manipulator (AREA)

Abstract

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

Description

一种基于三维点云模型的智能消防机器人控制方法
技术领域
本发明涉及变电站消防领域，具体涉及一种基于三维点云模型的智能消防机器人控制方法。
背景技术
目前现有的变压器水介质的消防系统多采用低压框架式水喷淋灭火系统，大多采用固定式喷头，安装复杂，占地面积大，喷头数量多，运维工作量大，协同效果差，喷水效果不佳，灭火效果不理想；并且各消防子系统都是独立的消防系统，存在集中管理程度低、无本地综合预警和远程交互功能、联动及时性差、信息系统不具备智能分析能力等问题，不符合国家电网“变电站细水雾灭火技术”要求，因此急需一种安全、经济、有效的消防灭火手段。同现有手段相比，应用了六轴机器人的智能消防系统具有设备结构小、施工成本低、灭火启动快、便于控制、及时消灭早期火灾、灭火由人工智能控制或人员远程参与控制、灭火人员远离火灾现场、安全性高，高压细水雾节约用水、灭火效果好，现场设备简单不影响主变日常维护检修等优点。
六自由度的机器人运动学反解非常复杂，一般没有封闭解。在应用D-H法建立运动学方程的基础上，进行一定的解析计算后发现，位置反解往往有很多个，不能得到有效的封闭解。
发明内容
针对目前技术存在的不足，本发明的目的在于提供一种基于三维点云模型的智能消防机器人控制方法，
此种控制方法将红外相机识别到的异常升温点的位置信息与三维点云模型进行比对，得到异常升温点的坐标位置，通过旋转平移矩阵和Pieper方法，自动解算出六轴消防机器人各关节角转动的角度，为六轴消防机器人的定位提供辅助。应用此种控制方法的智能消防机器人，配合立柱电机及高压细水雾泵组，能够实现对异常升温点的精准、快速降温，大大提高了变电站现场消防系统的安全性。
为了达到上述目的，本发明采用如下技术方案：基于三维点云模型的智能消防机器人控制方法，具体步骤如下：
步骤1、通过三维激光雷达现场扫描建立现场三维点云模型；
步骤2、建立机器人坐标系和三维点云模型的映射关系；
步骤3、将红外相机捕获到画面的异常升温点与三维点云模型进行比对，提取异常升温点坐标；
步骤4、确定机器人末端期待位姿，建立齐次坐标矩阵；
步骤5、使用Pieper方法求解机器人各轴关节角度，选择最小移动角度为最优解；
步骤6、控制机器人移动至相应位置，进行降温，直至温度降至告警阈值以下；
步骤7、结束降温，机器人复位。
具体地，步骤2中，机器人上电，将立柱电机降至立柱最底部；通过机器人示教器上的按钮手动调整各轴，使得机器人第二轴第三轴间的连杆垂直于立柱，第三轴第四轴连杆与立柱平行，第六轴的轴线z方向指向世界坐标系z轴的反方向，将此位置设置为机械臂各轴电机原点；将机械臂调整至最佳启动位姿待机，记录并保存此时机械臂位姿的关节偏移与世界坐标；确定立柱电机底座为三维点云模型的原点，六轴机械臂底座为机器人坐标系原点，建立机器人坐标系与三维点云模型的映射关系。
具体地，步骤4中，计算机器人的正运动学，由于机器人各关节参数已知，可求解其 末端坐标系{E}相对于基坐标系{B}的变换映射
Figure 71392DEST_PATH_IMAGE001
。
其中，关节坐标系i到i-1的变换关系为：
Figure 362434DEST_PATH_IMAGE002
在上式中，i为机器人各轴的编号，其取值范围为1到6；英文字母
Figure 188176DEST_PATH_IMAGE003
为 连杆长 度，表示关节轴i-1和关节轴i之间的公共法线长度；希腊字母
Figure 114544DEST_PATH_IMAGE004
为连杆扭转，表示关节轴 i相对于关节轴i-1绕它们的公共法线旋转的角度；
Figure 716468DEST_PATH_IMAGE005
为连杆偏移，表示关节i与关节i+1的 公共法线和它与关节i-1的公共法线沿这个关节轴的距离；
Figure 663565DEST_PATH_IMAGE006
为关节转角，表示关节i与关 节i+1的公共法线和它与关节i-1的公共法线绕这个关节轴的转角；
Figure 461756DEST_PATH_IMAGE007
表示坐标 轴
Figure 503399DEST_PATH_IMAGE008
旋转
Figure 234595DEST_PATH_IMAGE004
角度的姿态描述；
Figure 290276DEST_PATH_IMAGE009
表示坐标轴
Figure 106922DEST_PATH_IMAGE010
旋转
Figure 234147DEST_PATH_IMAGE006
角度的姿态描述；
Figure 678904DEST_PATH_IMAGE011
表示沿着坐标轴
Figure 233382DEST_PATH_IMAGE008
进行
Figure 599641DEST_PATH_IMAGE003
长度的平移变换；
Figure 107720DEST_PATH_IMAGE012
表示沿着坐标 轴
Figure 718568DEST_PATH_IMAGE010
进行
Figure 552270DEST_PATH_IMAGE005
长度的平移变换。其一般表达式为：
Figure 45306DEST_PATH_IMAGE013
上式中
Figure 701284DEST_PATH_IMAGE014
，
Figure 144070DEST_PATH_IMAGE015
，
Figure 243613DEST_PATH_IMAGE016
，
Figure 381202DEST_PATH_IMAGE017
，各参数含义均 与（1）式相同。将所有六个关节相邻关节之间的变换映射由上式表示，即可获得由末端关节 坐标{E}系到基座标系{B}的映射关系:
Figure 44134DEST_PATH_IMAGE018
其中
Figure 583568DEST_PATH_IMAGE019
、
Figure 447488DEST_PATH_IMAGE020
、
Figure 337952DEST_PATH_IMAGE021
、
Figure 883203DEST_PATH_IMAGE022
、
Figure 339461DEST_PATH_IMAGE023
、
Figure 905441DEST_PATH_IMAGE024
分别表示其下标所示坐标系到上标所示坐标系的齐次 坐标变换矩阵。
在三维点云模型与所建机器人坐标系的基础上，通过红外相机获得机器人可达空间外异常升温点与机器人基坐标系原点连线交机器人可达空间球面于一点，交点即为期望末端位置坐标点，作为平移向量计算的依据，取该交点处球面的法线作为末端坐标系z轴的正方向，对末端坐标系x轴y轴进行与末端z轴相同的旋转变换可得期望旋转矩阵，完成对机器人末端期望齐次变换矩阵的确定。
1)直线方程的确定：
首先是确定空间直线的方程，取机器人可达空间外一点和机器人基坐标系原点构造空间直线方程如下：
Figure 424147DEST_PATH_IMAGE025
其中，
Figure 271624DEST_PATH_IMAGE026
为参变量，
Figure 503760DEST_PATH_IMAGE027
为基坐标系原点
Figure 162012DEST_PATH_IMAGE028
，空间直线的方向向量
Figure 495910DEST_PATH_IMAGE029
可由机器人可达空间外一点即异常升温点坐标
Figure 586226DEST_PATH_IMAGE030
和机器人基坐标系 原点共同确定：
Figure 33388DEST_PATH_IMAGE031
所以最终确定的直线方程为：
Figure 285377DEST_PATH_IMAGE032
而机器人可达空间的球面方程为：
Figure 840993DEST_PATH_IMAGE033
其中R为六轴机器人可达空间最外层球壳的半径。
2)联立求交点：将上述直线方程同球面方程联立，可得关于参变量t的一元二次方程。
Figure 734999DEST_PATH_IMAGE034
解出t=±R，带入(6)式，即可求得由异常升温点和机器人基座标系原点确定的空 间直线与机器人可达空间球面方程的交点坐标在机器人基座标系中的表示
Figure 833405DEST_PATH_IMAGE035
。
末端位姿的确定：由式(5)，可得机器人末端关节空间z轴方向向量，分别求其关于 基座标系
Figure 725138DEST_PATH_IMAGE036
平面的投影及
Figure 174574DEST_PATH_IMAGE037
平面的投影，以同样的方法求得机器人初始位姿下末端z轴 分量在基坐标系下
Figure 895709DEST_PATH_IMAGE036
平面的投影及
Figure 848621DEST_PATH_IMAGE037
平面的投影，即可求得由初始位姿z轴到期望位姿 z轴的旋转矩阵，令初始位姿机器人末端x轴、y轴方向向量进行相同的旋转变换，即可获得 期望的末端x轴、y轴方向向量，完成对末端期望齐次变换矩阵的确定。
具体地，步骤5中，在已知机器人期望末端齐次变换矩阵的基础上，可以对机器人进行逆运动学求解，但是六自由度的机器人的运动学反解非常复杂，一般没有封闭解。在应用D-H法建立运动学方程的基础上，使用三轴相交的Pieper解法进行机器人运动学逆运算的求解：
当机器人的4、5、6轴轴线相交于一点时，最后三轴的原点均位于此交点上，该交点在基座标系中的位置坐标为：
Figure 35889DEST_PATH_IMAGE038
式中，
Figure 441463DEST_PATH_IMAGE039
为包含4、5、6轴坐标系原点在基座标系中的位置信息的矩阵，x、y、z 表示4、5、6轴坐标系原点在基座标系中的三维坐标。为包含4、5、6轴坐标系原点在第三轴坐 标系中的位置信息的矩阵。
Figure 270747DEST_PATH_IMAGE019
、
Figure 547008DEST_PATH_IMAGE020
、
Figure 842860DEST_PATH_IMAGE021
分别表示其下标所示关节轴坐标系到其上标所示关 节轴坐标系的齐次坐标变换矩阵，具体计算方法见式（2）。
当
Figure 1309DEST_PATH_IMAGE040
时，可由（2）式中的变换第四列求得最后三轴的交点坐标在基坐标系中的 表示：
Figure 165443DEST_PATH_IMAGE041
式中
Figure 92947DEST_PATH_IMAGE042
、
Figure 966226DEST_PATH_IMAGE043
、
Figure 346391DEST_PATH_IMAGE044
均表示其下标对应关节轴相关的DH参数，
Figure DEST_PATH_IMAGE045
，
Figure 251899DEST_PATH_IMAGE046
即
Figure 299490DEST_PATH_IMAGE047
。为了便于表示和计算，对上式进行数学代换：
Figure 343669DEST_PATH_IMAGE048
其中
Figure 100002_DEST_PATH_IMAGE049
、
Figure 866923DEST_PATH_IMAGE050
、
Figure 654751DEST_PATH_IMAGE051
为以第三轴关节转角
Figure 556848DEST_PATH_IMAGE052
为自变量的函数表达式，其具体含 义如下：
Figure 834245DEST_PATH_IMAGE053
根据式（2），得到上式中
Figure 48058DEST_PATH_IMAGE054
、
Figure 967472DEST_PATH_IMAGE055
、
Figure 458496DEST_PATH_IMAGE056
的表达式：
Figure 100002_DEST_PATH_IMAGE057
Figure 906795DEST_PATH_IMAGE058
Figure 748849DEST_PATH_IMAGE059
式中，
Figure 471955DEST_PATH_IMAGE060
、
Figure 817485DEST_PATH_IMAGE042
、
Figure 764582DEST_PATH_IMAGE061
、
Figure 93932DEST_PATH_IMAGE044
、
Figure 558411DEST_PATH_IMAGE062
、
Figure 820765DEST_PATH_IMAGE043
、
Figure 876446DEST_PATH_IMAGE052
分别表示其下标对应关节轴的DH参数，
Figure 100002_DEST_PATH_IMAGE063
表示
Figure 427513DEST_PATH_IMAGE064
，
Figure 961262DEST_PATH_IMAGE046
表示
Figure 343702DEST_PATH_IMAGE065
，
Figure 570284DEST_PATH_IMAGE066
表示
Figure 608647DEST_PATH_IMAGE067
，
Figure 883771DEST_PATH_IMAGE068
表示
Figure 917455DEST_PATH_IMAGE069
，
Figure 783780DEST_PATH_IMAGE070
分别表示
Figure 371756DEST_PATH_IMAGE071
与
Figure 247308DEST_PATH_IMAGE072
。同样 对于
Figure 807602DEST_PATH_IMAGE039
,应用式（2）可得：
Figure 48091DEST_PATH_IMAGE073
其中，
Figure 326625DEST_PATH_IMAGE074
、
Figure 271448DEST_PATH_IMAGE075
分别表示
Figure 420669DEST_PATH_IMAGE076
与
Figure 222272DEST_PATH_IMAGE077
，
Figure 722524DEST_PATH_IMAGE078
为第一关节轴转角。上式中
Figure 939878DEST_PATH_IMAGE079
Figure 146869DEST_PATH_IMAGE080
Figure 57056DEST_PATH_IMAGE081
式中，
Figure 779024DEST_PATH_IMAGE054
、
Figure 65649DEST_PATH_IMAGE055
、
Figure 251780DEST_PATH_IMAGE056
计算方式见式（13），
Figure 332868DEST_PATH_IMAGE082
、
Figure 479816DEST_PATH_IMAGE083
分别表示
Figure 38973DEST_PATH_IMAGE084
与
Figure 876348DEST_PATH_IMAGE085
，
Figure 128338DEST_PATH_IMAGE086
为第二关节轴 转角。其余参数与式（13）中相应参数含义相同。然后计算
Figure 824898DEST_PATH_IMAGE039
的坐标平方和：
Figure 100002_DEST_PATH_IMAGE087
由（8）可得：
Figure 335818DEST_PATH_IMAGE088
进行变量替换消去因变量
Figure 168645DEST_PATH_IMAGE078
，化简
Figure 529219DEST_PATH_IMAGE086
的关系式，同时写出矩阵
Figure 978655DEST_PATH_IMAGE039
z方向分量的 方程：
Figure 145194DEST_PATH_IMAGE089
Figure 98106DEST_PATH_IMAGE090
其中,
Figure 754216DEST_PATH_IMAGE091
Figure 159789DEST_PATH_IMAGE092
Figure 67702DEST_PATH_IMAGE093
Figure 875121DEST_PATH_IMAGE094
由式(18)求解第三关节轴关节转角
Figure 702132DEST_PATH_IMAGE052
，在此处分三种情况：
1)若
Figure 595002DEST_PATH_IMAGE095
=0，则r=
Figure 775447DEST_PATH_IMAGE096
，此处r已知而等式右侧的
Figure 968531DEST_PATH_IMAGE096
仅为关于
Figure 904126DEST_PATH_IMAGE052
的函数，用三角函数万 能公式进行变量替换后可求解。
2)若
Figure 815450DEST_PATH_IMAGE097
=0，则
Figure 127483DEST_PATH_IMAGE098
，此处
Figure 175073DEST_PATH_IMAGE099
已知，再次使用三角函数万能公式，利用变量替换即 可求解
Figure 281570DEST_PATH_IMAGE052
.
3)若上述两种情况都不满足，则利用方程组消去
Figure 414611DEST_PATH_IMAGE083
和
Figure 530334DEST_PATH_IMAGE082
，得到：
Figure 963590DEST_PATH_IMAGE100
带入三角函数万能公式，可得到一元四次方程，由此可解算出
Figure 506566DEST_PATH_IMAGE052
;
在成功解算出
Figure 330166DEST_PATH_IMAGE052
后，可由上述方程求解第二关节轴和第一关节轴转角
Figure 249580DEST_PATH_IMAGE086
和
Figure 802921DEST_PATH_IMAGE078
;
在解出前三关节轴转角
Figure 251220DEST_PATH_IMAGE078
、
Figure 30958DEST_PATH_IMAGE086
、
Figure 816380DEST_PATH_IMAGE052
后，由于第四、第五、第六轴的轴线相交于一点， 故这些关节角只影响末端连杆的方向。此处先令第四关节轴转角
Figure 99594DEST_PATH_IMAGE101
，计算出此时的连 杆坐标系{4}相对于基坐标系的旋转矩阵
Figure 984373DEST_PATH_IMAGE102
，坐标系{6}的期望方向与连杆坐标系{4}的区 别在于后三个关节的作用，由于
Figure 110461DEST_PATH_IMAGE103
已知，故可解出连杆坐标系{4}相对于机器人末端坐标 系的旋转矩阵
Figure 840519DEST_PATH_IMAGE104
：
Figure 100002_DEST_PATH_IMAGE105
对于绝大多数机械臂，可由
Figure 696349DEST_PATH_IMAGE106
欧拉角解法应用于第四关节轴
Figure 17609DEST_PATH_IMAGE101
时的
Figure 630993DEST_PATH_IMAGE104
解出第四、第五、第六轴的角度值。在所有解中寻找机器人各关节移动距离最短的解作 为最优解。
具体地，步骤6中：工控机将解算出的关节角度值发送给智能消防机器人，等待机器人运动至相应的异常温升点；机器人运动结束后，打开高压细水雾分区控制阀，对异常升温点进行喷洒降温，直至异常升温点温度降至告警阈值以下。
本发明应用Pieper方法，借助三维点云模型点云数据事先确定异常升温点的位置和机械臂运动至期望点的位姿，由异常升温点位置求取机械臂前三轴的关节转角，然后由机器人期望位姿计算得出机器人后三轴的关节转角，实现了对于六关节均为旋转关节且最后三轴相交的6自由度机器人封闭解的求取，相对于其他逆运动学求解方法，具有运算过程简单，运算速度快等优点。同时，将三维点云模型和红外相机监控画面相结合，可快速得到异常升温点的位置坐标，能够提高整个智能消防系统的响应速度。本发明所述的基于三维点云模型的智能消防机器人控制方法可应用在一般工控机上，配备红外摄像头辅助定位，可实现对异常升温点的精准、快速降温，大大提高了变电站现场消防系统的安全性。
附图说明
图1为本发明控制方法原理图。
具体实施方式：
下面结合智能消防机器人搭载的红外摄像头、滑杆电机、泵组分区控制阀做更详细的说明。参照图1，本发明的具体过程如下：
步骤1：通过三维激光雷达现场扫描建立现场三维点云模型：
三维激光雷达扫描得到变电站现场的激光点云数据，建立现场三维点云模型。
步骤2：建立机器人坐标系和三维点云模型的映射关系：
机器人上电，将立柱电机降至立柱最底部；通过机器人示教器上的按钮手动调整各轴，使得机器人第二轴第三轴间的连杆垂直于立柱，第三轴第四轴连杆与立柱平行，第六轴的轴线z方向指向世界坐标系z轴的反方向，将此位置设置为机械臂各轴电机原点。将机械臂调整至最佳启动位姿待机，记录并保存此时机械臂位姿的关节偏移与世界坐标。确定立柱电机底座为三维点云模型的原点，六轴机械臂底座为机器人坐标系原点，建立机器人坐标系与三维点云模型的映射关系。
步骤3：将红外相机捕获到画面的异常升温点与三维点云模型进行比对，提取异常升温点坐标；
步骤4：确定机器人末端期待位姿，建立齐次坐标矩阵：
计算六轴机器人的正运动学，由于机器人各关节参数已知，可求解其末端坐标系 {E}相对于基坐标系{B}的变换映射
Figure 695901DEST_PATH_IMAGE001
，本发明所应用六轴机器人D-H参数下表所示。
关节 杆长
Figure 100002_DEST_PATH_IMAGE107
 扭角
Figure 16024DEST_PATH_IMAGE108
 杆距d(mm) 转角
Figure 100002_DEST_PATH_IMAGE109
1 0 0 0
Figure 632819DEST_PATH_IMAGE110
2 70 90° 0
Figure 100002_DEST_PATH_IMAGE111
3 390 0 0
Figure 61395DEST_PATH_IMAGE112
4 125 90° 394
Figure 100002_DEST_PATH_IMAGE113
5 0 -90° 0
Figure 398835DEST_PATH_IMAGE114
6 0 90° 119
Figure 100002_DEST_PATH_IMAGE115
其中，关节坐标系i到i-1的变换关系为：
Figure 698098DEST_PATH_IMAGE116
在上式中，i为机器人各轴的编号，其取值范围为1到6；英文字母
Figure 157899DEST_PATH_IMAGE003
为 连杆长 度，表示关节轴i-1和关节轴i之间的公共法线长度；希腊字母
Figure 417979DEST_PATH_IMAGE004
为连杆扭转，表示关节轴 i相对于关节轴i-1绕它们的公共法线旋转的角度；
Figure 824689DEST_PATH_IMAGE005
为连杆偏移，表示关节i与关节i+1的 公共法线和它与关节i-1的公共法线沿这个关节轴的距离；
Figure 791508DEST_PATH_IMAGE006
为关节转角，表示关节i与关 节i+1的公共法线和它与关节i-1的公共法线绕这个关节轴的转角；表示坐标轴
Figure 156630DEST_PATH_IMAGE008
旋转
Figure 966323DEST_PATH_IMAGE004
角度的姿态描述；
Figure 645566DEST_PATH_IMAGE009
表示坐标轴
Figure 857105DEST_PATH_IMAGE010
旋转
Figure 330812DEST_PATH_IMAGE006
角度的姿态描述；
Figure 503167DEST_PATH_IMAGE011
表示 沿着坐标轴
Figure 251680DEST_PATH_IMAGE008
进行
Figure 255408DEST_PATH_IMAGE003
长度的平移变换；
Figure 962333DEST_PATH_IMAGE012
表示沿着坐标轴
Figure 418722DEST_PATH_IMAGE010
进行长度的平移 变换。其一般表达式为：
Figure 100002_DEST_PATH_IMAGE117
上式中
Figure 564402DEST_PATH_IMAGE014
，
Figure 688215DEST_PATH_IMAGE015
，
Figure 503725DEST_PATH_IMAGE016
，
Figure 736427DEST_PATH_IMAGE017
，各参数含义均 与（1）式相同。将所有六个关节相邻关节之间的变换映射由上式表示，即可获得由末端关节 坐标{E}系到基座标系{B}的映射关系:
Figure 623480DEST_PATH_IMAGE018
其中
Figure 601800DEST_PATH_IMAGE019
、
Figure 119369DEST_PATH_IMAGE020
、
Figure 753613DEST_PATH_IMAGE021
、
Figure 709937DEST_PATH_IMAGE022
、
Figure 808343DEST_PATH_IMAGE023
、
Figure 168917DEST_PATH_IMAGE024
分别表示其下表所示坐标系到上标所示坐标系的齐次 坐标变换矩阵。
期望末端齐次变换矩阵的确定：在所建世界坐标系与机器人坐标系的基础上，通过红外相机获得机器人可达空间外异常升温点与基坐标系原点连线交机器人可达空间球面于一点，交点即为期望末端位置坐标点，作为平移向量计算的依据，取该交点处球面的法线作为末端坐标系z轴的正方向，对末端坐标系x轴y轴进行与末端z轴相同的旋转变换，可得期望旋转矩阵，至此，完成对机器人末端期望齐次变换矩阵的确定。
1)直线方程的确定：
首先是确定空间直线的方程，取机器人可达空间外一点和机器人基坐标系原点构造空间直线方程如下：
Figure 352773DEST_PATH_IMAGE118
其中，
Figure 784892DEST_PATH_IMAGE027
为基坐标系原点
Figure 737804DEST_PATH_IMAGE028
，空间直线的方向向量
Figure 597176DEST_PATH_IMAGE029
可由 机器人可达空间外一点即异常升温点坐标
Figure 268329DEST_PATH_IMAGE030
和机器人基坐标系原点共同确定：
Figure 238559DEST_PATH_IMAGE031
故最终确定的直线方程为
Figure 100002_DEST_PATH_IMAGE119
而机器人可达空间的球面方程为：
Figure 967349DEST_PATH_IMAGE033
其中R为六轴机器人可达空间最外层球壳的半径。本发明所用六轴机器人R=1800mm。
2)联立求交点：将上述直线方程同球面方程联立，可得关于参变量t的一元二次方程。
Figure 669726DEST_PATH_IMAGE034
解出t=±R，带入(6)式，即可求得由异常升温点和机器人基座标系原点确定的空 间直线与机器人可达空间球面方程的交点坐标在机器人基座标系中的表示
Figure 828175DEST_PATH_IMAGE035
。
末端位姿的确定：由式(5)，可得机器人末端z轴方向向量，分别求其关于基座标系
Figure 602096DEST_PATH_IMAGE036
平面的投影及
Figure 529601DEST_PATH_IMAGE037
平面的投影，以同样的方法求得机器人初始位姿下末端z轴分量在基 坐标系下
Figure 730775DEST_PATH_IMAGE036
平面的投影及
Figure 907678DEST_PATH_IMAGE037
平面的投影，即可求得由初始位姿z轴到期望位姿z轴的旋 转矩阵，令初始位姿机器人末端x轴、y轴方向向量进行相同的旋转变换，即可获得期望的末 端x轴、y轴方向向量，完成对末端期望齐次变换矩阵的确定。
步骤5：使用Pieper方法求解机器人各轴关节角度，选择最小移动角度为最优解：
在已知机器人期望末端齐次变换矩阵的基础上，可以对机器人进行逆运动学求解，但是六自由度的机器人的运动学反解非常复杂，一般没有封闭解。在应用D-H法建立运动学方程的基础上，应用三轴相交的Pieper解法进行机器人运动学逆运算的求解：
当机器人的4、5、6轴轴线相交于一点时，最后三轴的原点均位于此交点上，该交点在基座标系中的位置坐标为：
Figure 485290DEST_PATH_IMAGE038
式中，
Figure 532880DEST_PATH_IMAGE039
为包含4、5、6轴坐标系原点在基座标系中的位置信息的矩阵，x、y、z 表示4、5、6轴坐标系原点在基座标系中的三维坐标。
Figure 904956DEST_PATH_IMAGE120
为包含4、5、6轴坐标系原点在第 三轴坐标系中的位置信息的矩阵。
Figure 241259DEST_PATH_IMAGE019
、
Figure 356983DEST_PATH_IMAGE020
、
Figure 259080DEST_PATH_IMAGE021
分别表示其下标所示关节轴坐标系到其上标 所示关节轴坐标系的齐次坐标变换矩阵，具体计算方法见式（2）。
当
Figure 536477DEST_PATH_IMAGE040
时，可由（2）式中的变换第四列求得最后三轴的交点坐标在基坐标系中的 表示：
Figure 953552DEST_PATH_IMAGE121
式中
Figure 607388DEST_PATH_IMAGE042
、
Figure 629570DEST_PATH_IMAGE043
、
Figure 77869DEST_PATH_IMAGE044
均表示其下标对应关节轴相关的DH参数，
Figure 592027DEST_PATH_IMAGE045
，
Figure 49553DEST_PATH_IMAGE046
即
Figure 660663DEST_PATH_IMAGE047
。为了便于表示和计算，对上式进行数学代换：
Figure 545442DEST_PATH_IMAGE122
其中
Figure 671530DEST_PATH_IMAGE049
、
Figure 932747DEST_PATH_IMAGE050
、
Figure 398364DEST_PATH_IMAGE051
为以第三轴关节转角
Figure 454044DEST_PATH_IMAGE052
为自变量的函数表达式，其具体含 义如下：
Figure 942795DEST_PATH_IMAGE123
根据式（2），得到上式中
Figure 742123DEST_PATH_IMAGE054
、
Figure 327825DEST_PATH_IMAGE055
、
Figure 554407DEST_PATH_IMAGE056
的表达式：
Figure 717404DEST_PATH_IMAGE124
Figure 320424DEST_PATH_IMAGE125
Figure 963895DEST_PATH_IMAGE126
式中，
Figure 626958DEST_PATH_IMAGE060
、
Figure 949354DEST_PATH_IMAGE042
、
Figure 559327DEST_PATH_IMAGE061
、
Figure 650780DEST_PATH_IMAGE044
、
Figure 484744DEST_PATH_IMAGE062
、
Figure 232120DEST_PATH_IMAGE043
、
Figure 176942DEST_PATH_IMAGE052
分别表示其下标对应关节轴的DH参数，
Figure 263847DEST_PATH_IMAGE063
表示
Figure 268712DEST_PATH_IMAGE064
，
Figure 768964DEST_PATH_IMAGE046
表示
Figure 517477DEST_PATH_IMAGE065
，
Figure 521205DEST_PATH_IMAGE066
表示
Figure 493709DEST_PATH_IMAGE067
，
Figure 481257DEST_PATH_IMAGE068
表示
Figure 502302DEST_PATH_IMAGE069
，
Figure 891695DEST_PATH_IMAGE070
分别表示
Figure 707204DEST_PATH_IMAGE071
与
Figure 182048DEST_PATH_IMAGE072
。同样 对于
Figure 905039DEST_PATH_IMAGE039
,应用式（2）可得：
Figure 414518DEST_PATH_IMAGE127
式中
Figure 932087DEST_PATH_IMAGE074
、
Figure 363068DEST_PATH_IMAGE075
分别表示
Figure 991496DEST_PATH_IMAGE076
与
Figure 152219DEST_PATH_IMAGE077
，
Figure 575110DEST_PATH_IMAGE078
为第一关节轴转角。其中
Figure 431070DEST_PATH_IMAGE128
Figure 863189DEST_PATH_IMAGE129
Figure 816101DEST_PATH_IMAGE130
式中，
Figure 472210DEST_PATH_IMAGE054
、
Figure 143363DEST_PATH_IMAGE055
、
Figure 848014DEST_PATH_IMAGE056
计算方式见式（13），
Figure 921012DEST_PATH_IMAGE082
、
Figure 623389DEST_PATH_IMAGE083
分别表示
Figure 109734DEST_PATH_IMAGE084
与
Figure 211551DEST_PATH_IMAGE085
，
Figure 342318DEST_PATH_IMAGE086
为第二关节轴 转角，其余参数与式（13）中相应参数含义相同。计算
Figure 340230DEST_PATH_IMAGE039
的坐标平方和：
Figure 985975DEST_PATH_IMAGE087
由（8）可得：
Figure 298007DEST_PATH_IMAGE088
进行变量替换消去因变量
Figure 752123DEST_PATH_IMAGE078
，化简
Figure 124198DEST_PATH_IMAGE086
的关系式，同时写出
Figure 257239DEST_PATH_IMAGE039
Z方向分量的方 程：
Figure 100002_DEST_PATH_IMAGE131
Figure 169700DEST_PATH_IMAGE132
其中,
Figure 868535DEST_PATH_IMAGE091
Figure 145933DEST_PATH_IMAGE133
Figure 766270DEST_PATH_IMAGE093
Figure 92209DEST_PATH_IMAGE134
由上式求解
Figure 583233DEST_PATH_IMAGE052
，在此处分三种情况：
1)若
Figure 562690DEST_PATH_IMAGE095
=0，则r=
Figure 201482DEST_PATH_IMAGE096
，此处r已知而等式右侧的
Figure 393429DEST_PATH_IMAGE096
仅为关于
Figure 942222DEST_PATH_IMAGE052
的函数，用三角函数万 能公式进行变量替换后可求解。
2)若
Figure 154898DEST_PATH_IMAGE097
=0，则
Figure 749827DEST_PATH_IMAGE098
，此处
Figure 745465DEST_PATH_IMAGE099
已知，再次使用三角函数万能公式，利用变量替换即 可求解
Figure 476660DEST_PATH_IMAGE052
.
3)若上述两种情况都不满足，则利用方程组消去
Figure 204445DEST_PATH_IMAGE083
和
Figure 755512DEST_PATH_IMAGE082
，得到：
Figure 351578DEST_PATH_IMAGE100
带入三角函数万能公式，可得到一元四次方程，由此可解算出
Figure 671701DEST_PATH_IMAGE052
.在成功解算出
Figure 835966DEST_PATH_IMAGE052
后，可由上述方程求解
Figure 874330DEST_PATH_IMAGE086
和
Figure 8508DEST_PATH_IMAGE078
.
Figure 714296DEST_PATH_IMAGE135
、
Figure 111779DEST_PATH_IMAGE136
、
Figure 100002_DEST_PATH_IMAGE137
的求解：
在解出前三关节轴转角
Figure 106280DEST_PATH_IMAGE078
、
Figure 247411DEST_PATH_IMAGE086
、
Figure 807705DEST_PATH_IMAGE052
后，由于第四、第五、第六轴的轴线相交于一点， 故这些关节角只影响末端连杆的方向。此处先令第四关节轴转角
Figure 438407DEST_PATH_IMAGE101
，计算出此时的连 杆坐标系{4}相对于基坐标系的旋转矩阵
Figure 185783DEST_PATH_IMAGE102
，坐标系{6}的期望方向与连杆坐标系{4}的区 别在于后三个关节的作用，由于
Figure 130605DEST_PATH_IMAGE103
已知，故可解出连杆坐标系{4}相对于机器人末端坐标 系的旋转矩阵
Figure 217510DEST_PATH_IMAGE104
：
Figure 222375DEST_PATH_IMAGE105
对于绝大多数机械臂，可由Z-Y-Z欧拉角解法应用于
Figure 50523DEST_PATH_IMAGE101
时的
Figure 533457DEST_PATH_IMAGE104
解出第四、第 五、第六轴的角度值。在所有解中寻找机器人各关节移动距离最短的解作为最优解。
步骤6：控制机器人移动至相应位置，进行降温，直至温度降至告警阈值以下：
工控机将解算出的关节角度值发送给智能消防机器人，等待机器人运动至相应的异常温升点。机器人运动结束后，打开高压细水雾分区控制阀，对异常升温点进行喷洒降温，直至异常升温点温度降至告警阈值以下。
步骤7：完成降温过程，机器人复位，等待下一次启动。
将本发明所述的智能消防机器人控制方法应用在一般工控机上，配备摄像头辅助定位，可实现对异常升温点的精准、快速降温，大大提高了变电站现场消防系统的安全性。

Claims (5)

1.基于三维点云模型的智能消防机器人控制方法，其特征在于步骤如下：
步骤1、通过三维激光雷达现场扫描建立现场三维点云模型；
步骤2、建立机器人坐标系和三维点云模型的映射关系；
步骤3、将红外相机捕获到画面的异常升温点与三维点云模型进行比对，提取异常升温点坐标；
步骤4、确定机器人末端期待位姿，建立齐次坐标矩阵；
步骤5、使用Pieper方法求解机器人各轴关节角度，选择最小移动角度为最优解；
步骤6、控制机器人移动至相应位置，进行降温，直至温度降至告警阈值以下；
步骤7、结束降温，机器人复位。
2.根据权利要求1所述的基于三维点云模型的智能消防机器人控制方法，其特征在于，步骤2中，机器人上电，将立柱电机降至立柱最底部；通过示教器上的按钮调整各轴，使得机器人第二轴第三轴间的连杆垂直于立柱，第三轴第四轴连杆与立柱平面，第六轴的轴线z方向指向世界坐标系z轴的反方向，将此位置设置为机械臂各轴电机原点；将机械臂调整至最佳启动位姿待机，记录并保存此时机械臂位姿的关节偏移与世界坐标；确定立柱电机底座为三维点云模型的原点，六轴机械臂底座为机器人坐标系原点，建立机器人坐标系与三维点云模型的映射关系。
3.根据权利要求1所述的基于三维点云模型的智能消防机器人控制方法，其特征在于，步骤4具体如下：
计算六轴机器人的正运动学，由于机器人各关节参数已知，可求解其末端坐标系{E}相 对于基坐标系{B}的变换映射
Figure DEST_PATH_IMAGE001
；
其中，关节坐标系
Figure DEST_PATH_IMAGE003
到
Figure 424692DEST_PATH_IMAGE004
的变换关系为：
Figure 774902DEST_PATH_IMAGE006
在上式中，i为机器人各轴的编号，其取值范围为1到6；英文字母
Figure DEST_PATH_IMAGE007
为 连杆长度，表 示关节轴i-1和关节轴i之间的公共法线长度；希腊字母
Figure 382994DEST_PATH_IMAGE008
为连杆扭转，表示关节轴i相对 于关节轴i-1绕它们的公共法线旋转的角度；
Figure DEST_PATH_IMAGE009
为连杆偏移，表示关节i与关节i+1的公共法 线和它与关节i-1的公共法线沿这个关节轴的距离；
Figure 705522DEST_PATH_IMAGE010
为关节转角，表示关节i与关节i+1的 公共法线和它与关节i-1的公共法线绕这个关节轴的转角；
Figure DEST_PATH_IMAGE011
表示坐标轴
Figure 269096DEST_PATH_IMAGE012
旋 转
Figure DEST_PATH_IMAGE013
角度的姿态描述；
Figure 44285DEST_PATH_IMAGE014
表示坐标轴
Figure DEST_PATH_IMAGE015
旋转
Figure 813658DEST_PATH_IMAGE010
角度的姿态描述；
Figure 831773DEST_PATH_IMAGE016
表 示沿着坐标轴
Figure 130030DEST_PATH_IMAGE012
进行
Figure DEST_PATH_IMAGE017
长度的平移变换；
Figure 126936DEST_PATH_IMAGE018
表示沿着坐标轴
Figure 932956DEST_PATH_IMAGE015
进行
Figure 761234DEST_PATH_IMAGE009
长度的 平移变换；
其一般表达式为：
Figure 495972DEST_PATH_IMAGE020
上式中
Figure DEST_PATH_IMAGE021
，
Figure 481639DEST_PATH_IMAGE022
，
Figure DEST_PATH_IMAGE023
，
Figure 796077DEST_PATH_IMAGE024
，各参数含义均与（1） 式相同；将所有六个关节相邻关节之间的变换映射由上式表示，即可获得由末端关节坐标 {E}系到基座标系{B}的映射关系:
Figure 744441DEST_PATH_IMAGE026
其中
Figure DEST_PATH_IMAGE027
、
Figure 289561DEST_PATH_IMAGE028
、
Figure DEST_PATH_IMAGE029
、
Figure 854534DEST_PATH_IMAGE030
、
Figure DEST_PATH_IMAGE031
、
Figure 942970DEST_PATH_IMAGE032
分别表示其下表所示坐标系到上标所示坐标系的齐次坐标 变换矩阵；
期望末端齐次变换矩阵的确定：在所建世界坐标系与机器人坐标系的基础上，通过红外相机获得机器人可达空间外异常升温点与基坐标系原点连线交机器人可达空间球面于一点，交点即为期望末端位置坐标点，作为平移向量计算的依据，取该交点处球面的法线作为末端坐标系z轴的正方向，对末端坐标系x轴y轴进行与末端z轴相同的旋转变换可得期望旋转矩阵，至此，完成对机器人末端期望齐次变换矩阵的确定；
1)直线方程的确定：
首先是确定空间直线的方程，取机器人可达空间外一点和机器人基坐标系原点构造空间直线方程如下：
Figure 745840DEST_PATH_IMAGE034
其中，
Figure DEST_PATH_IMAGE035
为参变量，
Figure 930703DEST_PATH_IMAGE036
为基坐标系原点
Figure DEST_PATH_IMAGE037
，空间直线的方向向量
Figure 717393DEST_PATH_IMAGE038
可由机器人可达空间外一点即异常升温点坐标
Figure DEST_PATH_IMAGE039
和机器人基坐标系 原点共同确定：
Figure 108055DEST_PATH_IMAGE040
故最终确定的直线方程为：
Figure 532476DEST_PATH_IMAGE042
而机器人可达空间的球面方程为：
Figure 248759DEST_PATH_IMAGE044
其中R为六轴机器人可达空间最外层球壳的半径；
2)联立求交点：将上述直线方程同球面方程联立，可得关于参变量t的一元二次方程：
Figure 585063DEST_PATH_IMAGE046
解出t=±R，带入(6)式，即可求得由异常升温点和机器人基座标系原点确定的空间直 线与机器人可达空间球面方程的交点坐标在机器人基座标系中的表示
Figure DEST_PATH_IMAGE047
；
末端位姿的确定：由式(5)，可得机器人末端z轴方向向量，分别求其关于基座标系
Figure 982677DEST_PATH_IMAGE048
平面的投影及
Figure DEST_PATH_IMAGE049
平面的投影，以同样的方法求得机器人初始位姿下末端z轴分量在基坐标 系下
Figure 789834DEST_PATH_IMAGE048
平面的投影及
Figure 473756DEST_PATH_IMAGE049
平面的投影，即可求得由初始位姿z轴到期望位姿z轴的旋转矩 阵，令初始位姿机器人末端x轴、y轴方向向量进行相同的旋转变换，即可获得期望的末端x 轴、y轴方向向量，完成对末端期望齐次变换矩阵的确定。
4.根据权利要求3所述的基于三维点云模型的智能消防机器人控制方法，其特征在于，步骤5具体如下：
当机器人的4、5、6轴轴线相交于一点时，最后三轴的原点均位于此交点上，该交点在基座标系中的位置坐标为：
Figure DEST_PATH_IMAGE051
式中，
Figure 110405DEST_PATH_IMAGE052
为包含4、5、6轴坐标系原点在基座标系中的位置信息的矩阵，x、y、z表示4、 5、6轴坐标系原点在基座标系中的三维坐标；
Figure DEST_PATH_IMAGE053
为包含4、5、6轴坐标系原点在第三轴坐 标系中的位置信息的矩阵；
Figure 926090DEST_PATH_IMAGE027
、
Figure 761322DEST_PATH_IMAGE028
、
Figure 616146DEST_PATH_IMAGE029
分别表示其下标所示关节轴坐标系到其上标所示关 节轴坐标系的齐次坐标变换矩阵，具体计算方法见式（2）；
当
Figure 192621DEST_PATH_IMAGE054
时，可由（2）式中的变换第四列求得最后三轴的交点坐标在基坐标系中的表示：
Figure 525513DEST_PATH_IMAGE056
式中
Figure DEST_PATH_IMAGE057
、
Figure 917049DEST_PATH_IMAGE058
、
Figure DEST_PATH_IMAGE059
均表示其下标对应关节轴相关的DH参数，
Figure 473932DEST_PATH_IMAGE060
，
Figure DEST_PATH_IMAGE061
；为了便于表示和计算，对上式进行数学代换：
Figure DEST_PATH_IMAGE063
其中
Figure 852217DEST_PATH_IMAGE064
、
Figure DEST_PATH_IMAGE065
、
Figure 395325DEST_PATH_IMAGE066
为以第三轴关节转角
Figure DEST_PATH_IMAGE067
为自变量的函数表达式，其具体含义如 下：
Figure DEST_PATH_IMAGE069
根据式（2），得到上式中
Figure 969264DEST_PATH_IMAGE070
、
Figure DEST_PATH_IMAGE071
、
Figure 572415DEST_PATH_IMAGE072
的表达式：
Figure 592323DEST_PATH_IMAGE074
Figure 532597DEST_PATH_IMAGE076
Figure 698393DEST_PATH_IMAGE078
式中，
Figure DEST_PATH_IMAGE079
、
Figure 597079DEST_PATH_IMAGE057
、
Figure 776387DEST_PATH_IMAGE080
、
Figure 458035DEST_PATH_IMAGE059
、
Figure DEST_PATH_IMAGE081
、、
Figure 68883DEST_PATH_IMAGE067
分别表示其下标对应关节轴的DH参数，
Figure 607312DEST_PATH_IMAGE082
表示
Figure DEST_PATH_IMAGE083
，
Figure 414862DEST_PATH_IMAGE084
表示
Figure DEST_PATH_IMAGE085
，
Figure 463983DEST_PATH_IMAGE086
表示
Figure DEST_PATH_IMAGE087
，
Figure 306168DEST_PATH_IMAGE088
表示
Figure DEST_PATH_IMAGE089
，
Figure 281077DEST_PATH_IMAGE090
分别表示
Figure DEST_PATH_IMAGE091
与
Figure 136776DEST_PATH_IMAGE092
；同样对于
Figure 160226DEST_PATH_IMAGE052
,应用式（2）可得：
Figure 450393DEST_PATH_IMAGE094
式中
Figure DEST_PATH_IMAGE095
、
Figure 374967DEST_PATH_IMAGE096
分别表示
Figure DEST_PATH_IMAGE097
与
Figure 157109DEST_PATH_IMAGE098
，
Figure DEST_PATH_IMAGE099
为第一关节轴转角，
Figure DEST_PATH_IMAGE101
Figure DEST_PATH_IMAGE103
Figure DEST_PATH_IMAGE105
其中，
Figure 92573DEST_PATH_IMAGE070
、
Figure 830722DEST_PATH_IMAGE071
、
Figure 881854DEST_PATH_IMAGE072
计算方式见式（13），、
Figure 449495DEST_PATH_IMAGE106
分别表示
Figure DEST_PATH_IMAGE107
与
Figure 142645DEST_PATH_IMAGE108
，
Figure DEST_PATH_IMAGE109
为第二关节轴转角； 其余参数与式（13）中相应参数含义相同；计算
Figure 813929DEST_PATH_IMAGE052
的坐标平方和：
Figure DEST_PATH_IMAGE111
由（8）可得：
Figure DEST_PATH_IMAGE113
进行变量替换消去因变量
Figure 268919DEST_PATH_IMAGE099
，化简
Figure 87970DEST_PATH_IMAGE109
的关系式，同时写出
Figure 115969DEST_PATH_IMAGE052
Z方向分量的方程：
Figure DEST_PATH_IMAGE115
Figure DEST_PATH_IMAGE117
其中,
Figure DEST_PATH_IMAGE119
Figure DEST_PATH_IMAGE121
Figure DEST_PATH_IMAGE123
Figure DEST_PATH_IMAGE125
由式(18)求解第三关节轴关节转角
Figure 392492DEST_PATH_IMAGE067
，在此处分三种情况：
若
Figure 51006DEST_PATH_IMAGE126
=0，则r=
Figure DEST_PATH_IMAGE127
，此处r已知而等式右侧的
Figure 419671DEST_PATH_IMAGE127
仅为关于
Figure 126727DEST_PATH_IMAGE067
的函数，用三角函数万能公式 进行变量替换后可求解；
若
Figure 867543DEST_PATH_IMAGE128
=0，则
Figure DEST_PATH_IMAGE129
，此处
Figure 962538DEST_PATH_IMAGE130
已知，再次使用三角函数万能公式，利用变量替换即可求解
Figure 287340DEST_PATH_IMAGE067
；
若上述两种情况都不满足，则利用方程组消去
Figure 532508DEST_PATH_IMAGE106
和
Figure DEST_PATH_IMAGE131
，得到：
Figure 656059DEST_PATH_IMAGE132
代入三角函数万能公式，可得到一元四次方程，由此可解算出
Figure 656376DEST_PATH_IMAGE067
；
在成功解算出
Figure 406158DEST_PATH_IMAGE067
后，可由上述方程求解第二关节轴和第一关节轴转角
Figure 517333DEST_PATH_IMAGE109
和
Figure 221023DEST_PATH_IMAGE099
；
Figure DEST_PATH_IMAGE133
、
Figure 657820DEST_PATH_IMAGE134
、
Figure DEST_PATH_IMAGE135
的求解：
在解出前三关节轴转角
Figure 98160DEST_PATH_IMAGE099
、
Figure 809764DEST_PATH_IMAGE109
、
Figure 642329DEST_PATH_IMAGE067
后，由于第四、第五、第六轴的轴线相交于一点，故这 些关节角只影响末端连杆的方向；此处先令第四关节轴转角
Figure 922131DEST_PATH_IMAGE136
，计算出此时的连杆坐 标系{4}相对于基坐标系的旋转矩阵
Figure DEST_PATH_IMAGE137
，坐标系{6}的期望方向与连杆坐标系{4}的区别在 于后三个关节的作用，由于
Figure 974401DEST_PATH_IMAGE138
已知，故可解出：
Figure 161800DEST_PATH_IMAGE140
对于绝大多数机械臂，可由Z-Y-Z欧拉角解法应用于
Figure 851801DEST_PATH_IMAGE136
时的
Figure DEST_PATH_IMAGE141
解出第四、第五、第 六轴的角度值；
在所有解中寻找机器人各关节移动距离最短的解作为最优解。
5.根据权利要求1所述的基于三维点云模型的智能消防机器人控制方法，其特征在于，步骤6中，工控机将解算出的关节角度值发送给智能消防机器人，等待机器人运动至相应的异常温升点；机器人运动结束后，打开高压细水雾分区控制阀，对异常升温点进行喷洒降温，直至异常升温点温度降至告警阈值以下。
CN202110285775.8A 2021-03-17 2021-03-17 一种基于三维点云模型的智能消防机器人控制方法 Pending CN113101584A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110285775.8A CN113101584A (zh) 2021-03-17 2021-03-17 一种基于三维点云模型的智能消防机器人控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110285775.8A CN113101584A (zh) 2021-03-17 2021-03-17 一种基于三维点云模型的智能消防机器人控制方法

Publications (1)

Publication Number Publication Date
CN113101584A true CN113101584A (zh) 2021-07-13

Family

ID=76711972

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110285775.8A Pending CN113101584A (zh) 2021-03-17 2021-03-17 一种基于三维点云模型的智能消防机器人控制方法

Country Status (1)

Country Link
CN (1) CN113101584A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114668995A (zh) * 2022-04-29 2022-06-28 西安交通大学 一种基于高压细水雾的变电站智能机器人消防系统及方法
WO2023083056A1 (zh) * 2021-11-12 2023-05-19 华为技术有限公司 用于标定机器人的运动学参数的方法和装置
CN116673966A (zh) * 2023-08-02 2023-09-01 北京迁移科技有限公司 用于机器人的关节角度生成方法及机器人系统

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2000061236A1 (en) * 1999-04-12 2000-10-19 Domenico Piatti Manually or remotely controlled, telescopic, fire-fighting robot,automatically connected to the water pipe system and hung on monorail in tunnels
CN103256421A (zh) * 2013-03-07 2013-08-21 荆门市广恒机电设备有限公司 高压细水雾灭火系统电磁分区控制阀
CN106980751A (zh) * 2017-02-27 2017-07-25 浙江大学 一种含双c轴的六轴自动化制孔锪窝机床的运动学反解方法
CN107275960A (zh) * 2017-05-10 2017-10-20 国网浙江省电力公司台州供电公司 一种电力故障应急专用机器人
CN209108481U (zh) * 2018-11-20 2019-07-16 姚文涛 吊轨式主动防御智能报警灭火机器人
CN110420421A (zh) * 2019-07-12 2019-11-08 东南大学 一种电缆通道巡检灭火机器人
CN111672045A (zh) * 2020-05-21 2020-09-18 国网湖南省电力有限公司 一种消防机器人、消防系统和消防控制方法
CN111890364A (zh) * 2020-07-29 2020-11-06 库卡机器人(广东)有限公司 机器人的运动控制方法、装置、电子设备及存储介质
CN112066994A (zh) * 2020-09-28 2020-12-11 河海大学常州校区 一种消防机器人局部自主导航方法及系统

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2000061236A1 (en) * 1999-04-12 2000-10-19 Domenico Piatti Manually or remotely controlled, telescopic, fire-fighting robot,automatically connected to the water pipe system and hung on monorail in tunnels
CN103256421A (zh) * 2013-03-07 2013-08-21 荆门市广恒机电设备有限公司 高压细水雾灭火系统电磁分区控制阀
CN106980751A (zh) * 2017-02-27 2017-07-25 浙江大学 一种含双c轴的六轴自动化制孔锪窝机床的运动学反解方法
CN107275960A (zh) * 2017-05-10 2017-10-20 国网浙江省电力公司台州供电公司 一种电力故障应急专用机器人
CN209108481U (zh) * 2018-11-20 2019-07-16 姚文涛 吊轨式主动防御智能报警灭火机器人
CN110420421A (zh) * 2019-07-12 2019-11-08 东南大学 一种电缆通道巡检灭火机器人
CN111672045A (zh) * 2020-05-21 2020-09-18 国网湖南省电力有限公司 一种消防机器人、消防系统和消防控制方法
CN111890364A (zh) * 2020-07-29 2020-11-06 库卡机器人(广东)有限公司 机器人的运动控制方法、装置、电子设备及存储介质
CN112066994A (zh) * 2020-09-28 2020-12-11 河海大学常州校区 一种消防机器人局部自主导航方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
郝丽娜等: "《工业机器人控制技术》", 30 November 2018, 华中科技大学出版社 *
黄茂林: "《机械原理》", 31 July 2010, 机械工业出版社 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023083056A1 (zh) * 2021-11-12 2023-05-19 华为技术有限公司 用于标定机器人的运动学参数的方法和装置
CN114668995A (zh) * 2022-04-29 2022-06-28 西安交通大学 一种基于高压细水雾的变电站智能机器人消防系统及方法
CN116673966A (zh) * 2023-08-02 2023-09-01 北京迁移科技有限公司 用于机器人的关节角度生成方法及机器人系统
CN116673966B (zh) * 2023-08-02 2023-10-03 北京迁移科技有限公司 用于机器人的关节角度生成方法及机器人系统

Similar Documents

Publication Publication Date Title
CN113101584A (zh) 一种基于三维点云模型的智能消防机器人控制方法
CN108481323B (zh) 基于增强现实的机器人运动轨迹自动编程系统及方法
CN107756408A (zh) 一种基于主动红外双目视觉的机器人轨迹示教装置及方法
CN110695988A (zh) 双机械臂协同运动方法及系统
CN107901041A (zh) 一种基于图像混合矩的机器人视觉伺服控制方法
CN112925346A (zh) 基于人在回路的高层消防多无人机容错控制方法及系统
CN109571481A (zh) 一种关节奇异点处理方法、装置、设备和存储介质
CN111230866B (zh) 一种六轴机器人末端跟随目标物实时位姿的计算方法
CN108582031A (zh) 一种基于力反馈主从控制的带电作业机器人支接引线搭接方法
WO2015166574A1 (ja) 多関節ロボットのティーチングデータ作成装置及び作成方法
CN111015681A (zh) 信通机房巡检机器人系统
CN117817667B (zh) 一种基于svd分解法的机械臂末端姿态调整方法
Zeng et al. Autonomous control design of an unmanned aerial manipulator for contact inspection
Truong et al. Dynamic social zone for human safety in human-robot shared workspaces
季旭全 et al. Intelligent robotic assembly method of spaceborne equipment based on visual guidance
CN116330267A (zh) 一种基于工业机器人腕部奇异点计算的控制方法
CN114589701A (zh) 一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法
CN108427429B (zh) 一种考虑动态指向约束的航天器视轴机动控制方法
JP2007144538A (ja) ロボットのティーチングデータ作成方法
Chen et al. Intelligent power distribution live‐line operation robot systems based on stereo camera
CN107962562A (zh) 一种多自由度机械臂视觉伺服系统组建方法
CN117972925A (zh) 一种面向冗余空间机械臂的奇异性分析方法
CN112917479A (zh) 五轴机器人的近似位姿计算方法、装置和存储介质
CN112894819B (zh) 一种基于双神经网络的机器人动力学运动控制方法及装置
CN114610047A (zh) 一种在线深度估计的qmm-mpc水下机器人视觉对接控制方法

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
CB02 Change of applicant information

Address after: 330096 No. 88, Minqiang Road, private science and Technology Park, Qingshanhu District, Nanchang City, Jiangxi Province

Applicant after: STATE GRID JIANGXI ELECTRIC POWER COMPANY LIMITED Research Institute

Applicant after: STATE GRID JIANGXI ELECTRIC POWER Co.,Ltd.

Applicant after: STATE GRID CORPORATION OF CHINA

Address before: 330096 No.88 Minqiang Road, private science and Technology Park, high tech Zone, Nanchang City, Jiangxi Province

Applicant before: STATE GRID JIANGXI ELECTRIC POWER COMPANY LIMITED Research Institute

Applicant before: STATE GRID JIANGXI ELECTRIC POWER Co.,Ltd.

Applicant before: STATE GRID CORPORATION OF CHINA

CB02 Change of applicant information
RJ01 Rejection of invention patent application after publication

Application publication date: 20210713

RJ01 Rejection of invention patent application after publication