[发明专利]自行走式地下掘进机器人姿态测量装置及其测量方法

权利要求书

1.一种自行走式地下掘进机器人姿态测量装置，其特征在于：包括两组发射装置、至少三组接收装置和上位机；两组发射装置的组成电路相同，含有电子罗盘电路、电源电路、键盘输入设备、整平基座、控制器、无线电通讯电路、激光发射调制电路、旋转平台驱动电路；多组发射装置的组成电路相同，含有激光接收电路、无线通讯电路、坐标解算器；其中发射装置中的控制器的相应端口分别与电子罗盘电路、电源电路、键盘输入设备、整平基座、无线电通讯电路、激光发射调制电路、旋转平台驱动电路互连；接收装置中的坐标解算器相应端口分别与激光接收电路、无线通讯电路互连。

2.应用权利要求1所述自行走式地下掘进机器人姿态测量装置的测量方法，其特征在于：该方法基于前方交会原理，利用光束之间的几何关系、发射器的旋转速度，位置信息和光束被接收的时间差来计算接收器即目标点所处的空间坐标；依次含有以下步骤：

步骤1：在两组控制器中设定旋转平台的角速度ω_k，其中k=A,B)；和其空间坐标(x_k,y_k,z_k)，其中k=A,B)；各控制器对其发射器角速度和位置信息进行编码，输出TTL高低电平给激光发射调制电路，让其发射出带有各自特征信息的激光信号；

步骤2：接收器先后接收到两组发射器所发射的选通光信号，坐标解算器解调，根据不同发射器拥有不同角速度特征判断各自选通光被接收时刻，并将时刻和相应的角速度信息打包发送；

步骤3：所述两组控制器根据步骤2所得信息，比较各自的角速度与数据包中的是否相符，相符则接收，不相符则丢弃；然后根据该时刻信息读取对应时刻的罗盘角度信息；得到水平偏移角α_A，α_B；

步骤4：偏移角计算完毕后，接收器又先后接收两组发射器四束扇形光束的激光信号，坐标解算器记录四束光的先后时间，同时解调激光信号，获得不同发射器，不同时间的两束扇形光束被接受时刻信息

步骤5：所述坐标解算器根据步骤4所得信息计算各自发射器中两束扇形光被接收器接收的时间差

步骤6：所述坐标解算器根据步骤5所获得的信息计算接收器相对于发射器所在平面的俯仰角β_A，β_B；

其中θ是两束扇形光水平面上的夹角为90°，是两束扇形光与垂直面的夹角为30°；ω_A是A发射器的旋转角速度；ω_B是B发射器的旋转角速度；且ω_A≠ω_B；为A发射器中的两束扇形光被接收器接收时刻的时间差；为B发射器中的两束扇形光被接收器接收时刻的时间差；

步骤7：所述坐标解算器根据步骤3、步骤6所获得的角度信息和步骤2解调出的发射器位置信息计算接收器的空间坐标；

x=12b[1+sin(αB-αA)sin(αB+αA)]y=bsinαBtanβAsin(αB+αA)z=bsinαAsinαBsin(αA+αB)]]>

其中b=x_B-x_A；x_A，x_B分别为A，B发射器所在空间坐标系中x轴上的值；α_A，α_B分别为A，B发射器所测得的水平偏移角；β_A为A发射器测得的俯仰角；

步骤8：所述多个坐标解算器通过各自端口互连的无线通讯模块将各自接收器的空间坐标发送给上位机，上位机进行掘进机器人位姿计算。