[发明专利]一种基于差速轮组AGV的导航纠偏方法

权利要求书

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°。