CN112379666B - 一种基于卫星组合导航信息的斗臂车调斗作业引导方法 - Google Patents

Abstract

本发明公开了一种基于卫星组合导航信息的斗臂车调斗作业引导方法，包括步骤：(1)在机器人上安装组合导航设备，分别记录下在初始位置和目标作业位置的的经纬度坐标信息；(2)转换到斗臂车坐标系下得到初始位置和目标作业位置机器人在斗臂车坐标系下的坐标；(3)机器人建模并计算得到各目标电缆的最佳作业位置在斗臂车坐标系下的坐标；(4)建立以斗臂车大臂旋转中心为原点的球坐标系，分别得到初始位置和各目标电缆的最佳作业位置在球坐标系中的半径、仰角、方位角和切向角；(5)将二者的半径、仰角、方位角和切向角进行对比，指导斗臂车的移斗操作。本发明能够有效指导移斗操作，将平均移斗时间缩短到1分钟，精度达到6～8cm。

Description

一种基于卫星组合导航信息的斗臂车调斗作业引导方法

技术领域

本发明涉及绝缘斗臂车领域，尤其涉及一种基于卫星组合导航信息的引导斗臂车车斗到达最佳作业位置的方法。

背景技术

随着经济的发展，社会对供电持续可靠性要求不断提高，要求电网实现不间断输电。供电设备及线路在运行中需经常进行检修和维修，带电作业是避免检修停电，保证正常供电的有效措施。现有带电检修方式为：以绝缘斗臂车作为载体，在绝缘斗臂车的作业臂上安装工作斗，将带电作业机器人设置在工作斗内进行带电作业。

但是目前市面上的斗臂车无法返回车斗在空间中的三维坐标位置，无法指导车斗到达作业位置。组合导航设备可以安装在斗臂车上用于返回车斗在空间中的经纬度和高度，以获得最佳作业位置，但是经纬度和高度是一类直角坐标系，而斗臂的移动是一个近似球体运动轨迹的球坐标，纯粹依靠组合导航数据无法指导操作人员将车斗调至最佳作业位置。

发明内容

发明目的：本发明针对上述不足，提出了一种基于卫星组合导航信息的引导斗臂车车斗到达最佳作业位置的方法。

技术方案：

一种基于卫星组合导航信息的斗臂车调斗作业引导方法，包括步骤：

(1)在机器人上安装组合导航设备，在开始作业之前，将车斗放置在斗臂车的初始位置，通过组合导航设备记录下在该初始位置的组合导航设备的经纬度坐标信息；其中，经纬度坐标信息包括该初始位置的经纬度、高度和航向角信息，航向角信息指的是主天线指向从天线的方向；

(2)以斗臂车大臂旋转中心为原点，以大臂旋转中心指向车尾方向为x轴建立斗臂车坐标系，将初始位置的组合导航设备的经纬度坐标信息转换到斗臂车坐标系下，得到初始位置机器人在斗臂车坐标系下的坐标；

(3)将斗臂车的车斗升起到目标作业位置，通过组合导航设备获取在目标作业位置其经纬度坐标和高度并转换到斗臂车坐标系下得到目标作业位置机器人在斗臂车坐标系下的坐标；

(4)机器人对目标电缆进行建模，并计算并转换至斗臂车坐标系下得到各目标电缆的最佳作业位置在斗臂车坐标系下的坐标；

(5)建立以斗臂车大臂旋转中心为原点的球坐标系，根据步骤(2)和步骤(4)得到初始位置和各目标电缆的最佳作业位置在球坐标系中的坐标，并分别得到初始位置和各目标电缆的最佳作业位置在球坐标系中的半径、仰角、方位角和切向角；其中切向角即为机器人正面在球坐标系中与球面的切角；

(6)通过步骤(5)得到的球坐标参数，将初始位置和各目标电缆的最佳作业位置在球坐标系中的半径、仰角、方位角和切向角进行对比，指导斗臂车的移斗操作。

所述步骤(2)具体如下：

(21)测量在斗臂车坐标系下斗臂车大臂旋转中心到组合导航设备的主天线中心在X、Y、Z方向上的偏移量；

(22)将初始位置的组合导航设备的经纬度坐标信息转换到utm坐标系下；

(23)将utm坐标系下的初始位置的组合导航设备的坐标变换到以标定时刻的主天线为中心、主天线指向从天线方向为y轴的直角坐标系下，变换公式如下：

其中

是主天线的坐标，/>

是当前主天线的utm坐标，/>

是标定时刻主天线的utm坐标；

(24)将步骤(23)变换后得到的以标定时刻的主天线为中心、主天线指向从天线方向为y轴的直角坐标系下的初始位置的组合导航设备的坐标转换到以主天线为中心、大臂旋转中心指向车尾方向为y轴的直角坐标系，转换公式如下：

rz_antena＝rz_{utm_cur}-rz_{utm_calib}+rz_{antena_to_car}

其中，T_{rz_antena}是旋转矩阵，rz_antena是天线的旋转角度，rz_{utm_cur}是当前天线偏航角，rz_{utm_calib}是标定时刻天线偏航角，rz_{antena_to_car}是标定时刻天线偏航角和斗臂车坐标系y轴之间的夹角；

(25)将步骤(24)变换后得到的以主天线为中心、大臂旋转中心指向车尾方向为y轴的直角坐标系下的初始位置的组合导航设备的坐标变换到斗臂车坐标系下，得到初始位置机器人在斗臂车坐标系下的坐标，转换公式如下：

其中

是机器人在斗臂车坐标系下的坐标，/>

是测量的组合导航设备的主天线和机器人之间x,y,z三个方向上的偏移量，/>

是测量的组合导航设备的主天线到大臂旋转中心的偏移量。

所述步骤(5)中初始位置P_{origin_position}[r,θ,φ,cutting]或各目标电缆的最佳作业位置P_{target_position}[r,θ,φ,cutting]计算具体如下：

θ＝arcsin(z₀/r)

φ＝arctan(y₀/x₀)

cutting＝rz₀-φ

其中，x₀，y₀，z₀为机器人在球坐标系下的坐标，r为球坐标半径，θ为球坐标仰角，

为球坐标方位角，cutting为切向角，rz₀为偏航角，偏航角表示组合导航设备的主天线的朝向，即机器人的朝向，用偏航角减去方位角，则表示为机器人面向的方向与球坐标方位角0度之间的夹角。

有益效果：本发明的方法能够有效指导移斗操作，将平均移斗时间缩短到1分钟。通过移斗调整到最佳作业位置的调斗精度达到6～8cm。

附图说明

图1为本发明的大臂旋转中心的结构示意图。

图2为本发明坐标系变换流程图。

图3为本发明方法流程图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

图1为本发明的大臂旋转中心的结构示意图。如图1所示，本发明基于卫星组合导航信息引导斗臂车到达最佳作业位置的方法包括步骤：

(1)在机器人上安装组合导航设备，在开始作业之前，将车斗放置在斗臂车的初始位置，通过组合导航设备记录下在该初始位置的组合导航设备的经纬度坐标信息V_gps[altitude,longtitude,latitude,head]，其中该初始位置的组合导航设备的经纬度坐标信息V_gps[altitude,longtitude,latitude,head]包括该初始位置的经纬度、高度和航向角信息，航向角信息指的是主天线指向从天线的方向；以斗臂车大臂旋转中心为原点，以大臂旋转中心指向车尾方向为x轴，竖直方向为z轴，满足右手定则建立斗臂车坐标系，测量在斗臂车坐标系下斗臂车大臂旋转中心到组合导航设备的主天线中心在X、Y、Z方向上的偏移量V_{offset_1}[x,y,z]；

(2)将初始位置的组合导航设备的经纬度坐标信息转换到斗臂车坐标系下，得到初始位置机器人(即车斗)在斗臂车坐标系下的坐标；转换方法如下：

(21)将初始位置的组合导航设备的经纬度坐标信息转换到utm坐标系下，utm坐标也是一个平面坐标系，与经纬度有一一对应的关系；

(22)将utm坐标系下的初始位置的组合导航设备的坐标变换到以标定时刻的主天线为中心、主天线指向从天线方向为y轴的直角坐标系下，变换公式如下：

其中

是主天线的坐标，/>

是当前主天线的utm坐标，/>

是标定时刻主天线的utm坐标。

(23)将步骤(22)变换后得到的以标定时刻的主天线为中心、主天线指向从天线方向为y轴的直角坐标系下的初始位置的组合导航设备的坐标转换到以主天线为中心、大臂旋转中心指向车尾方向为y轴的直角坐标系，转换公式如下：

rz_antena＝rz_{utm_cur}-rz_{utm_calib}+rz_{antena_to_car}

其中，T_{rz_antena}是旋转矩阵，rz_antena是天线的旋转角度，rz_{utm_cur}是当前天线偏航角，rz_{utm_calib}是标定时刻天线偏航角，rz_{antena_to_car}是标定时刻天线偏航角和斗臂车坐标系y轴之间的夹角。

(24)将步骤(23)变换后得到的以主天线为中心、大臂旋转中心指向车尾方向为y轴的直角坐标系下的初始位置的组合导航设备的坐标变换到斗臂车坐标系下，得到初始位置机器人在斗臂车坐标系下的坐标，转换公式如下：

其中

是机器人在斗臂车坐标系下的坐标，/>

是测量的组合导航设备的主天线和机器人之间x,y,z三个方向上的偏移量，/>

是测量的组合导航设备的主天线到大臂旋转中心的偏移量，即前文中的V_{offset_1}[x,y,z]。

最终，得到初始位置机器人(即车斗)在斗臂车坐标系下的坐标P_robot[x,y,z]，即

(3)将斗臂车的车斗升起到目标作业位置，通过组合导航设备获取在目标作业位置其经纬度坐标和高度并转换到斗臂车坐标系下得到目标作业位置机器人在斗臂车坐标系下的坐标；

(4)机器人对目标电缆进行建模，计算并转换至斗臂车坐标系下得到各目标电缆(三相主线)的最佳作业位置在斗臂车坐标系下的坐标；

(5)建立以斗臂车大臂旋转中心为原点的球坐标系，根据步骤(2)和步骤(4)得到初始位置和各目标电缆的最佳作业位置在球坐标系中的坐标；其中，初始位置的球坐标P_{origin_position}[r,θ,φ,cutting]和各目标电缆的最佳作业位置的球坐标P_{target_position}[r,θ,φ,cutting]，计算公式如下：

θ＝arcsin(z₀/r)

φ＝arctan(y₀/x₀)

cutting＝rz₀-φ

其中，x₀，y₀，z₀为机器人在球坐标系下的坐标，r为球坐标半径，θ为球坐标仰角，

为球坐标方位角，cutting为切向角，rz₀为偏航角，偏航角表示组合导航设备的主天线的朝向，即机器人的朝向，用偏航角减去方位角，则表示为机器人面向的方向与球坐标方位角0度之间的夹角。

(6)通过步骤(5)得到的球坐标参数，将初始位置和各目标电缆的最佳作业位置在球坐标系中的半径、仰角、方位角和切向角进行对比，可以直观的反映并指导斗臂车手臂的移斗操作，相对半径对应小臂伸缩，当各目标电缆的最佳作业位置的r偏大时，小臂伸出，偏小时小臂缩回；相对仰角对应大臂的升降，当各目标电缆的最佳作业位置θ偏大时，大臂升高，反之降低；相对偏航角对应大臂的旋转，当各目标电缆的最佳作业位置

偏大时，大臂逆时针旋转，反之顺时针旋转；相对切向角对应车斗的旋转，当各目标电缆的最佳作业位置cutting偏大时，车斗角度顺时针旋转，反之逆时针旋转，相对切向角对应车斗的旋转。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。

Claims (2)

1.一种基于卫星组合导航信息的斗臂车调斗作业引导方法，其特征在于：包括步骤：

(1)在机器人上安装组合导航设备，在开始作业之前，将车斗放置在斗臂车的初始位置，通过组合导航设备记录下在该初始位置的组合导航设备的经纬度坐标信息；其中，经纬度坐标信息包括该初始位置的经纬度、高度和航向角信息，航向角信息指的是主天线指向从天线的方向；

(2)以斗臂车大臂旋转中心为原点，以大臂旋转中心指向车尾方向为x轴建立斗臂车坐标系，将初始位置的组合导航设备的经纬度坐标信息转换到斗臂车坐标系下，得到初始位置机器人在斗臂车坐标系下的坐标；

具体如下：

(21)测量在斗臂车坐标系下斗臂车大臂旋转中心到组合导航设备的主天线中心在X、Y、Z方向上的偏移量；

(22)将初始位置的组合导航设备的经纬度坐标信息转换到utm坐标系下；

(23)将utm坐标系下的初始位置的组合导航设备的坐标变换到以标定时刻的主天线为中心、主天线指向从天线方向为y轴的直角坐标系下，变换公式如下：

其中

是主天线的坐标，/>

是当前主天线的utm坐标，/>

是标定时刻主天线的utm坐标；

(24)将步骤(23)变换后得到的以标定时刻的主天线为中心、主天线指向从天线方向为y轴的直角坐标系下的初始位置的组合导航设备的坐标转换到以主天线为中心、大臂旋转中心指向车尾方向为y轴的直角坐标系，转换公式如下：

rz_antena＝rz_{utm_cur}-rz_{utm_calib}+rz_{antena_to_car}

其中，T_{rz_antena}是旋转矩阵，rz_antena是天线的旋转角度，rz_{utm_cur}是当前天线偏航角，rz_{utm_calib}是标定时刻天线偏航角，rz_{antena_to_car}是标定时刻天线偏航角和斗臂车坐标系y轴之间的夹角；

(25)将步骤(24)变换后得到的以主天线为中心、大臂旋转中心指向车尾方向为y轴的直角坐标系下的初始位置的组合导航设备的坐标变换到斗臂车坐标系下，得到初始位置机器人在斗臂车坐标系下的坐标，转换公式如下：

其中

是机器人在斗臂车坐标系下的坐标，/>

是测量的组合导航设备的主天线和机器人之间x,y,z三个方向上的偏移量，/>

是测量的组合导航设备的主天线到大臂旋转中心的偏移量；

(3)将斗臂车的车斗升起到目标作业位置，通过组合导航设备获取在目标作业位置其经纬度坐标和高度并转换到斗臂车坐标系下得到目标作业位置机器人在斗臂车坐标系下的坐标；

(4)机器人对目标电缆进行建模，并计算并转换至斗臂车坐标系下得到各目标电缆的最佳作业位置在斗臂车坐标系下的坐标；

(5)建立以斗臂车大臂旋转中心为原点的球坐标系，根据步骤(2)和步骤(4)得到初始位置和各目标电缆的最佳作业位置在球坐标系中的坐标，并分别得到初始位置和各目标电缆的最佳作业位置在球坐标系中的半径、仰角、方位角和切向角；其中切向角即为机器人正面在球坐标系中与球面的切角；

(6)通过步骤(5)得到的球坐标参数，将初始位置和各目标电缆的最佳作业位置在球坐标系中的半径、仰角、方位角和切向角进行对比，指导斗臂车的移斗操作。

2.根据权利要求1所述的斗臂车调斗作业引导方法，其特征在于：所述步骤(5)中初始位置P_{origin_position}[r,θ,φ,cutting]或各目标电缆的最佳作业位置P_{target_position}[r,θ,φ,cutting]计算具体如下：

θ＝arcsin(z₀/r)

φ＝arctan(y₀/x₀)

cutting＝rz₀-φ

其中，x₀，y₀，z₀为机器人在球坐标系下的坐标，r为球坐标半径，θ为球坐标仰角，

为球坐标方位角，cutting为切向角，rz₀为偏航角，偏航角表示组合导航设备的主天线的朝向，即机器人的朝向，用偏航角减去方位角，则表示为机器人面向的方向与球坐标方位角0度之间的夹角。/>

Images