[發明專利]一種基于衛星組合導航信息的斗臂車調斗作業引導方法

有效

• 申請號: 202011042320.5
• 申請日: 2020-09-28
• 公開（公告）號: CN112379666B
• 公開（公告）日: 2023-06-23
• 發明（設計）人: 程敏;王松;李栗;何靜
• 申請（專利權）人: 億嘉和科技股份有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02;G01S19/48;G01C21/20
• 代理公司: 南京瑞弘專利商標事務所(普通合伙) 32249
• 代理人: 梁天彥

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度之間的夾角。