[發明專利]一種基于車體位姿的作業部件末端位姿測量方法

有效

• 申請號: 202110188180.0
• 申請日: 2021-02-18
• 公開（公告）號: CN112987069B
• 公開（公告）日: 2021-11-26
• 發明（設計）人: 胡煉;賴桑愉;何杰;羅錫文;陳高隆;黃浩;劉海龍;高銳濤
• 申請（專利權）人: 華南農業大學
• 主分類號: G01S19/53
• 分類號: G01S19/53
• 代理公司: 廣州市華學知識產權代理有限公司 44245
• 代理人: 付茵茵

1.一種基于車體位姿的作業部件末端位姿測量方法，其特征在于：包括如下步驟：

a、將姿態傳感器安裝于車體或與車體剛性連接的載體平臺上，測量車體姿態；

b、將GNSS系統中的衛星天線安裝于車體頂部，測量車體的位姿信息；或將激光傳感安裝于車體或載體平臺上，測量基于基準作業面的相對位置信息；

c、建立車體坐標系Oxyz，以車體前進方向為y軸，x軸與y軸垂直指向車體方向的右側，z軸垂直于x、y軸向上；

d、在各個執行機構安裝傳感器，檢測執行機構的姿態變化以及位置變化，并建立作業部件坐標系Ox_ny_nz_n，n＝1,2...；

e、獲取衛星天線或激光傳感的位置信息，獲取車體姿態信息，獲取執行機構的姿態信息及位置變化量，建立基于所獲取信息及執行機構幾何關系求解作業部件末端中心的位置解算模型，并計算獲得作業部件末端中心坐標；

f、測量作業部件的姿態信息并推算作業部件末端的姿態信息；

g、設計基準作業面，結合所求的作業部件末端位姿信息計算作業部件末端與基準作業面的高程差Δh，以及作業部件末端與基準作業面的角度差Δα；

步驟e中，根據衛星天線的位置信息、車體姿態信息和各執行機構的姿態信息及位置變化量，建立GNSS坐標解算模型，解算作業部件末端三維坐標，GNSS坐標解算模型如下：

其中，n＝1,2,...；r表示作業部件末端中心的三維坐標；p表示衛星天線的位置信息；ψ,θ分別為車體偏航角、橫滾角和俯仰角；β₁,β₂,......,β_n為各執行機構上姿態傳感器測量值；l₁,l₂,......,l_n為各執行機構上位移傳感器測量值；

解算作業部件末端三維坐標步驟如下：

(1)測量車體及執行機構的角度與長度信息和衛星天線至車體質心位置的坐標增量，輸入GNSS坐標解算模型；

(2)換算車體姿態角；

(3)計算車身姿態旋轉矩陣；

(4)根據幾何關系或機器人運動學及所測量的各執行機構姿態信息與位置變化量，解算基于車體坐標系的作業部件末端中心的位置信息；

(5)計算衛星天線至作業部件末端中心的坐標增量；

(6)結合步驟(5)及衛星天線的位置信息，計算作業部件末端中心的三維坐標；

或步驟e中，根據激光傳感的位置信息、車體姿態信息和各執行機構的姿態信息及位置變化量，建立激光高程解算模型，求解作業部件末端中心的高程信息，激光高程解算模型如下：

其中，n＝1,2,...；H表示作業部件末端中心相對于基準作業面的相對位置信息；h_p表示激光接收器相對于基準作業面的相對高程；ψ,θ分別為車體偏航角、橫滾角和俯仰角；β₁,β₂,......,β_n為各執行機構上姿態傳感器測量值；l₁,l₂,......,l_n為各執行機構上位移傳感器測量值；

作業部件末端中心高程解算步驟如下：

(1)測量車體及執行機構的角度與長度信息和激光接收器至車體或載體平臺姿態傳感器安裝位置的Z軸坐標增量，輸入激光高程解算模型；

(2)計算車體姿態旋轉矩陣；

(3)根據幾何關系或機器人運動學及所測量的各執行機構姿態信息與位置變化量，解算基于車體坐標系的作業部件末端中心的坐標Z軸分量；

(4)計算激光接收器至作業部件末端中心的Z軸坐標增量；

(5)結合步驟(4)及激光傳感的位置信息，計算作業部件末端中心相對于基準作業面的相對位置信息。

2.按照權利要求1所述的一種基于車體位姿的作業部件末端位姿測量方法，其特征在于：步驟b中，GNSS系統用于計算偏航角ψ及空間位置坐標。

3.按照權利要求1所述的一種基于車體位姿的作業部件末端位姿測量方法，其特征在于：步驟d中，根據執行機構的運動需求，傳感器選擇姿態傳感器或位移傳感器。