[發明專利]一種基于差速輪組AGV的導航糾偏方法

有效

• 申請號: 202010842964.6
• 申請日: 2020-08-20
• 公開（公告）號: CN111930126B
• 公開（公告）日: 2021-06-01
• 發明（設計）人: 黎成杰;劉文鵬;段三軍;李明
• 申請（專利權）人: 北京特種機械研究所
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京天方智力知識產權代理事務所(普通合伙) 11719
• 代理人: 張廷利

1.一種基于差速輪組AGV的導航糾偏方法，所述AGV安裝有四個呈中心對稱分布的差速輪組，差速輪組設有對稱分布的左驅動輪和右驅動輪，AGV還安裝有前后對稱分布的前磁導航傳感器和后磁導航傳感器，其特征在于，所述導航糾偏方法包括以下步驟：

S1、以AGV自身坐標系為車身坐標系，車中心為原點，正前方為Y方向，正右方為X方向；給定AGV的Y方向速度Vy；

S2、通過前磁導航傳感器和后磁導航傳感器實時采集導航磁條信息，通過絕對值編碼器實時檢測各差速輪組的當前航向角；

S3、根據前磁導航傳感器和后磁導航傳感器的位置，計算AGV中心相對導航磁條的偏移量DP和航向偏角AP，航向偏角AP是指Vy方向與導航磁條之間形成的夾角；

S4、根據偏移量DP和航向偏角AP計算糾偏速度Vx和糾偏轉速w，Vx方向與Vy方向垂直，w是指AGV繞中心的旋轉速度；

S5、采用速度分解法將Vy、Vx和w分解到各差速輪組，以得到各差速輪組的目標速度和目標航向角；

S6、根據各自的目標速度、目標航向角和當前航向角計算各差速輪組左驅動輪和右驅動輪的目標角速度；

S7、根據各差速輪組左驅動輪和右驅動輪的目標角速度控制對應驅動電機的轉速；

步驟S4中，所述糾偏速度Vx和糾偏轉速w按以下公式計算獲得，

Vx＝DP/DP_MAX×Max_Vx (1)

w＝AP/AP_MAX×Max_w (2)

公式(1)、(2)中，DP_MAX為最大偏移量，AP_MAX為最大航向偏角，Max_Vx和Max_w為根據Vy設立的可變比例因子；

步驟S5中，所述各差速輪組的目標速度和目標航向角按以下公式計算獲得，

Vx_m＝Vx+y_m×w (5)

Vy_m＝Vy+x_m×w (6)

α_m＝atan(Vx_m/Vy_m) (7)

公式(5)、(6)、(7)中，Vx_m和Vy_m為各差速輪組的目標速度，α_m為各差速輪組的目標航向角，x_m和y_m為各差速輪組中心的坐標；

步驟S6中，所述各差速輪組左驅動輪和右驅動輪的目標角速度按以下公式計算獲得，

Speed_m＝Vy_m/r (8)

dspeed_m＝Speed_m×Scale_m (9)

Speed_m_1＝Speed_m+dspeed_m (10)

Speed_m_2＝Speed_m-dspeed_m (11)

公式(8)、(9)、(10)、(11)中，Speed_m為基準角速度，dspeed_m為差速值，Scale_m為差速比例因子，Speed_m_1和Speed_m_2為目標角速度，當AGV頭部偏左時，左驅動輪的目標角速度為Speed_m_1，右驅動輪的目標角速度為Speed_m_2，當AGV頭偏右時，左驅動輪的目標角速度為Speed_m_2，右驅動輪的目標角速度為Speed_m_1。

2.根據權利要求1所述的一種基于差速輪組AGV的導航糾偏方法，其特征在于，所述Max_Vx和Max_w按以下公式計算獲得，

Max_Vx＝tan(AP_MAX/180×π)×Vy (3)

Max_w＝tan(AP_MAX/180×π)×Vy/(y-x×(AP_MAX/180×π)) (4)

公式(4)中，x和y為各差速輪組中心坐標的絕對值。

3.根據權利要求2所述的一種基于差速輪組AGV的導航糾偏方法，其特征在于，所述DP_MAX為L/2，L為磁導航傳感器的長度，所述AP_MAX為3～5°。