一种惯导重载AGV系统及其控制方法与流程

本发明涉及到agv技术领域，特指一种惯导重载agv系统及其控制方法。

背景技术：

agv(automatedguidevehicle，自动导引运输车)是指装备有电磁或光学等自动导引装置的运输车，无需人工参与就可以随着生产工艺流程的调整而及时调整相应的运输路线，由agv代替人工运输，可以有效地降低工作人员的劳动强度，提高工作效率，因此在制造、电子、物流等行业得到了广泛的应用。

由于磁条导航对车体的运动学模型相关性较小，因此当前重载agv中通常采用磁条导航方式。但是在地面环境较为恶劣的工厂，如各类零件检修或加工工厂，地面上的碎屑等杂物以及进出的叉车会磨损或破坏地面上预先铺设的磁条，降低导航精度或导致导航失败。

技术实现要素：

针对上述不足，本发明解决的技术问题在于提供一种惯导重载agv系统及其控制方法，对双差速运动控制模型进行了简化，采用主动轮跟踪目标轨迹，从动轮跟踪主动轮的控制方法，避免惯性导航方式与双差速驱动结合时需考虑复杂车体模型的问题，同时可适用于嵌入式平台。

本发明的目的是通过以下技术方案实现的：

一种惯导重载agv系统，所述惯导重载agv系统包括：驱动装置组、控制模块、惯性导航模块和agv车体；

所述驱动装置组包括前轮驱动装置组和后轮驱动装置组；所述前轮驱动装置组包括左前轮驱动装置和右前轮驱动装置；所述后轮驱动装置组包括左后轮驱动装置和右后轮驱动装置；所述左前轮驱动装置和所述右前轮驱动装置安装在前轮驱动装置连接机构的左右两端；所述前轮驱动装置连接机构中心对称地水平安装在所述agv车体前端下方；所述前轮驱动装置连接机构和所述agv车体的前轮减震系统的导向轴通过前轮销轴连接；所述左后轮驱动装置和所述右后轮驱动装置安装在后轮驱动装置连接机构的左右两端；所述后轮驱动装置连接机构中心对称地水平安装在所述agv车体后端下方；所述后轮驱动装置连接机构和所述agv车体的后轮减震系统的导向轴通过后轮销轴连接；所述前轮减震系统的导向轴和所述后轮减震系统的导向轴均可以上下移动和水平转动；

所述驱动装置的电机上均安装有速度编码器，用于采集所述驱动装置中的驱动轮的速度信息，并发送到所述控制模块；所述前轮减震系统的导向轴上安装有角度编码器，用于获得前轮驱动装置组竖直方向与所述agv车体竖直方向的前轮夹角信息，并发送到所述控制模块；所述agv车体竖直方向为所述agv车体前后的中轴线平行的方向，所述前轮驱动装置组的竖直方向为垂直于所述前轮驱动装置连接机构的方向；所述后轮减震系统的导向轴上安装有角度编码器，用于获得所述后轮驱动装置组竖直方向与所述agv车体竖直方向的后轮夹角信息，并发送到所述控制模块；所述后轮驱动装置组的竖直方向为垂直于所述后轮驱动装置连接机构的方向；所述控制模块通过所述驱动装置组为驱动轮设置差速控制所述agv车体转向；

所述惯性导航模块包括磁尺传感器组、n组磁钉组和陀螺仪，n≥1；每组磁钉组包含m个磁钉，m≥2；所述n组磁钉组用于指示所述惯性导航模块的目标轨迹；所述磁尺传感器组包括前端磁尺传感器和后端磁尺传感器；所述前端磁尺传感器安装在所述agv车体的前端；所述后端磁尺传感器安装在所述agv车体的后端；当所述agv车体前进时，所述前端磁尺传感器检测计算磁钉位置信号，并发送到所述控制模块；当所述agv车体后退时，所述后端磁尺传感器检测计算磁钉位置信号，并发送到所述控制模块；所述磁钉位置信号包括当磁尺传感器经过磁钉时，所述磁钉相对磁尺中心的位置；所述陀螺仪安装在所述agv车体的原地旋转中心，用于采集陀螺仪信号，所述陀螺仪信号为所述agv车体相对于所述agv开机上电启动时的实时方位角，并发送到所述控制模块；所述控制模块接收所述磁钉位置信号、所述陀螺仪信号、所述驱动装置组中的车轮组的速度信息、前轮夹角信息和后轮夹角信息，控制所述驱动装置组带动所述agv车体沿磁钉组铺设方向和中心前进；

所述磁尺传感器组检测到所述磁钉位置信号后将所述磁钉位置信号发送给所述控制模块，所述控制模块将所述agv切换到惯性导航标定模式，对所述agv车体的方向和位置进行标定；在惯性导航标定模式下，所述磁尺传感器组未检测到所述磁钉位置信号后将未检测到所述磁钉位置信号发送给所述控制模块，所述控制模块将所述agv切换到惯性导航调整模式，调整所述agv车体沿着所述目标轨迹行走。

优选地，所述n组磁钉组中任意相邻的两个磁钉之间的距离大于所述磁尺传感器可检测到单个磁钉位置信号时磁尺与磁钉之间最大距离的两倍。

优选地，每组磁钉组中所述m个磁钉之间的距离是相等的。

优选地，m=2。

一种惯导重载agv系统的控制方法，包括惯性导航标定模式标定方法和惯性导航调整模式控制方法；当所述agv前进时，所述前轮驱动装置组为主动轮驱动装置组，所述后轮驱动装置组为从动轮驱动装置组；当所述agv后退时，所述后轮驱动装置组为主动轮驱动装置组，所述前轮驱动装置组为从动轮驱动装置组；所述主动轮驱动装置组跟踪所述目标轨迹，所述从动轮驱动装置组跟随所述主动轮驱动装置组的方向；

在所述惯性导航标定模式下，使用所述惯性导航标定模式标定方法对所述agv车体的方向和位置进行标定，所述惯性导航标定模式标定方法包括以下步骤：

1）所述agv向前运动，经过一组磁钉组中的第i个磁钉的时候，记录当前di值；如果i=1，所述agv继续前进；如果i≥2，进入步骤2）；

其中，di为所述磁尺传感器组检测到的第i个磁钉相对磁尺中心的位置，磁尺左边方向为负值，磁尺右边方向为正值；

2）根据公式（1）-（2）计算所述agv车体的方向与所述目标轨迹的方向间的偏差角度δθi：

δθi=arcsin((di-1-di)/li)（1）

其中：

di-1：所述磁尺传感器组检测到的第i-1个磁钉相对磁尺中心的位置，磁尺左边方向为负值，磁尺右边方向为正值；

li：所述第i个磁钉与所述第i-1个磁钉之间的距离；

δθi：所述agv车体经过所述第i个磁钉时所述agv车体的方向相对于所述目标轨迹的方向的偏差角度；

3）判断i是否等于m，如果是，进入步骤4）；如果否，回到步骤1）；

4）根据公式（2）计算δθi的平均值δθ：

δθ=/(m-1)（2）

5）根据公式（3）计算所述目标轨迹的方向在陀螺仪大地坐标系中相对所述陀螺仪初始方位角的角度θ0；

θ0=θc+δθ（3）

陀螺仪大地坐标系：所述陀螺仪在大地平面内的旋转坐标系，以所述agv开机上电启动时所述陀螺仪的初始方位角为0度，逆时针方向为正；

θc：当所述agv车体经过第m个磁钉时所述陀螺仪的信号，用于表示所述agv车体在所述陀螺仪大地坐标系中的实时方向；

6）根据公式（4）计算当所述agv车体经过第m个磁钉时所述主动轮驱动装置组中心点a到所述目标轨迹的垂直距离δx0，所述目标轨迹前进方向右边为正：

δx0=dm*cosδθ-d*sinδθ（4）

其中：

d：所述主动轮驱动装置组中心点a与检测所述磁钉位置信号的磁尺传感器的垂直距离；

在所述惯性导航调整模式下，使用所述惯性导航调整模式控制方法调整所述agv沿着目标轨迹行走，包括以下步骤：

1)所述控制模块等待定时器采样信号，若无，则继续等待；若有，则进入步骤2）；

2）所述控制模块采集所述陀螺仪信号θc、所述主动轮驱动装置组的左右两个驱动轮的实时速度信息vfl和vfr、所述驱动装置组中的主动轮夹角信息θf和从动轮夹角信息θr；其中，θf为所述主动轮驱动装置组竖直方向与所述agv车体竖直方向的实时夹角，顺时针方向为正，由所述主动轮驱动装置组上的角度编码器实时测量获得；θr为所述从动轮驱动装置组竖直方向与所述agv车体竖直方向的实时夹角，顺时针方向为正，由所述从动轮驱动装置组上的角度编码器实时测量获得；

3）所述控制模块计算所述主动轮驱动装置组的左右两个驱动轮的目标控制速度vfl、vfr，包括如下步骤：

a）所述控制模块计算|θ0-θc+θf|，如果|θ0-θc+θf|≤第一预设值，则设置所述主动轮驱动装置组中左右两侧驱动轮的转速差控制量δvf=0，进入步骤e）；如果|θ0-θc+θf|＞所述第一预设值，则进入步骤b）；所述第一预设值为所述主动轮驱动装置组竖直方向相对目标轨迹方向的偏移角度阈值；

b）根据公式（5）-（6）计算转速差控制分量δvf1、δvf2：

δvf1=-k1*(θ0-θc)（5）

δvf2=-k2*θf（6）

其中，k1为pid回路中的第二预设值；k2为pid回路中的第三预设值；

c）根据公式（7）-（10）计算转速差控制分量δvf3：

vf=（vfl+vfr）/2（7）

δx=δx0+σδt*sin(θ0-θc+θf)*vf（8）

θδx=arcsin(-k3*δx)（9）

δvf3=k1*θδx（10）

其中，k3为pid回路中的第四预设值，δx为在每一段计算周期δt内所述主动轮驱动装置组中心点a在垂直于所述目标轨迹方向上的位置的积分，vf为所述主动轮驱动装置组中心点a的实时速度，vfl、vfr为所述主动轮驱动装置组的左右两个驱动轮的实时速度，由所述主动轮驱动装置组上的速度编码器实时测量获得；

d）根据公式（11）计算所述主动轮驱动装置组中左右两个驱动轮的转速差控制量δvf；

δvf=δvf1+δvf2+δvf3（11）

e）根据公式（12）计算所述主动轮驱动装置组中心点a的目标控制速度vf，所述agv车体运动方向为正：

vf=v/cosθf（12）

其中，v为所述agv沿所述目标轨迹方向的预设的运动速度；

f）根据公式（13）-（14）计算所述主动轮驱动装置组的左右两个驱动轮的目标控制速度vfl、vfr：

vfl=vf+δvf（13）

vfr=vf-δvf（14）

4）所述控制模块计算所述从动轮驱动装置组的左右两个驱动轮的目标控制速度vrl和vrr，包括如下步骤：

a）所述控制模块计算|θ0-θc+θr|，如果|θ0-θc+θr|≤第五预设值，则设置所述从动轮驱动装置组中左右两侧驱动轮的转速差控制量δvr=0，进入步骤c）；如果|θ0-θc+θr|＞所述第五预设值，则进入步骤b）；所述第五预设值为所述从动轮驱动装置组竖直方向相对目标轨迹方向的偏移角度阈值；

b）根据公式（15）-（17）计算所述从动轮驱动装置组中左右两侧驱动轮的转速差控制量δvr：

δvr1=-k4*(θ0-θc)（15）

δvr2=-k5*θr（16）

δvr=δvr1+δvr2（17）

c）根据公式（18）计算所述从动轮驱动装置组中心点b的目标控制速度vr，所述agv车体运动方向为正：

vr=v/cosθr（18）

d）根据公式（19）-（20）计算所述从动轮驱动装置组的左右两个驱动轮的目标控制速度vrl和vrr：

vrl=vr+δvr（19）

vrr=vr-δvr（20）

5）所述控制模块将步骤3）和4）中计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述主动轮、从动轮驱动装置组；

6）所述主动轮、从动轮驱动装置组根据所述电机转速控制电机，调整所述agv车体的车身方向和位置；

7）进入步骤1）。

优选地，所述第一预设值与所述第五预设值相等。

优选地，k1=k2，k4=k5。

优选地，k1=k4，k2=k5。

优选地，所述惯导重载agv系统还包括原地旋转控制模式，所述原地旋转控制模式包括如下步骤：

1）所述控制模块接收到调度系统发送的原地旋转命令；

2）所述控制模块控制所述agv停车；

3）所述控制模块调整所述主动轮、从动轮驱动装置组的方向，包括如下步骤：

a)所述控制模块等待定时器采样信号，若无，则继续等待；若有，则进入步骤b）；

b）所述控制模块采集所述驱动装置组中的主动轮夹角信息θf和从动轮夹角信息θr；

c）所述控制模块计算|θf-90°|，如果|θf-90°|≤第六预设值，所述第六预设值为所述主动轮驱动装置组竖直方向相对所述agv车体竖直方向的偏移角度阈值，进行步骤4）；如果|θf-90°|＞所述第六预设值，进行下一步：

d）根据公式（21）和（22）计算δvf和δvr：

δvf=-k6*（θf-90°）（21）

δvr=-k6*（θr-90°）（22）

其中，k6为pid回路中的第七预设值；

e）根据公式（23）-（26）计算所述驱动装置组（1）的四个驱动轮的目标控制速度vfl、vfr、vrl和vrr：

vfl=δvf（23）

vfr=-δvf（24）

vrl=δvr（25）

vrr=-δvr（26）

f）所述控制模块将步骤e）计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述驱动装置组；

g）所述驱动装置组根据所述电机转速控制电机，调整所述主动轮、从动轮驱动装置组的方向；

h）进入步骤3）的步骤a）；

4）所述控制模块匀速旋转所述agv车体，包括如下步骤：

a)所述控制模块等待定时器采样信号，若无，则继续等待；若有，则进入步骤b）；

b)所述控制模块采集所述陀螺仪信号并记录为θc旋转前，用于表示所述agv车体开始旋转前在所述陀螺仪大地坐标系中的方位角；

c)所述控制模块根据公式（27）计算所述agv车体在所述陀螺仪大地坐标系中要旋转到的目标角度θtarget：

θtarget=θc旋转前+θturn（27）

其中，θturn为预设的相对旋转角度；

d）所述控制模块等待定时器采样信号，若无，则继续等待；若有，则进入步骤e）；

e）所述控制模块采集当前时刻所述陀螺仪信号θc；

f）所述控制模块根据公式（28）计算所述agv车体在所述陀螺仪大地坐标系中的当前角度与所述要旋转到的目标角度的差值δθremain：

δθremain=θc-θtarget（28）

g）所述控制模块对步骤f）计算的δθremain进行判断，如果-sign(θturn)*δθremain＜δθturn，进行步骤4）；如果-sign(θturn)*δθremain≥δθturn，进行下一步；其中，δθturn为第七预设值；

h）根据公式（21）-（22）计算δvf和δvr；根据公式（29）~（33）计算四个驱动轮的目标控制速度vfl、vfr、vrl和vrr：

vfl=ωr1+∆vf（29）

vfr=ωr2-∆vf（30）

vrl=-ωr2+∆vr（31）

vrr=-ωr1-∆vr（32）

ω=ω0（33）

其中：

ω：所述agv车体的目标旋转角速度，顺时针方向为正；

ω0：所述agv车体的预设旋转角速度，ω0为第八预设值，顺时针方向为正；

r1：所述主动轮驱动装置组竖直方向与所述agv车体竖直方向垂直时外侧主动轮到所述agv车体中心的距离；

r2：所述主动轮驱动装置组竖直方向与所述agv车体竖直方向垂直时内侧主动轮到所述agv车体中心的距离；

i）所述控制模块将步骤h）计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述驱动装置组；

j）所述驱动装置组根据所述电机转速控制电机，调整所述agv车体的车身方向；

k）进入步骤a）；

5）所述控制模块控制所述agv车体减速旋转直至停止，包括如下步骤：

a)所述控制模块等待定时器采样信号，若无，则继续等待；若有，则进入步骤b）；

b）所述控制模块采集当前时刻的所述陀螺仪信号θc；

c）所述控制模块根据公式（28）计算δθremain；

d）所述控制模块判断是否δθremain≤第九预设值，如果是，则停止所述agv车体，旋转结束，进入步骤5）；如果否，则进入步骤e）；

e）所述控制模块根据公式（34）计算目标旋转角速度ω值：

ω=ω1+k7*δθremain(34)

其中，k7为第十预设值，ω1为第十一预设值；

f）所述控制模块根据公式（29）~（32）和步骤e）中计算的ω计算四个驱动轮速度的目标控制速度vfl、vfr、vrl和vrr；

g）所述控制模块将步骤f）计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述驱动装置组；

h）所述驱动装置组根据所述电机转速控制电机，调整所述agv车体的车身方向；

i）进入步骤a）；

6）所述控制模块调整所述主动轮、从动轮驱动装置组的方向，包括如下步骤：

a)所述控制模块等待定时器采样信号，若无，则继续等待；若有，则进入步骤b）；

b）所述控制模块采集所述驱动装置组中的主动轮夹角信息θf和从动轮夹角信息θr；

c）所述控制模块计算|θf|，如果|θf|≤第十二预设值，则原地旋转完成，退出原地旋转控制模式，所述第十二预设值为主动轮驱动装置组竖直方向相对所述agv车体竖直方向的偏移角度阈值；如果|θf|＞所述第十二预设值，进入步骤b）；

d）所述控制模块根据公式（35）和（36）计算δvf和δvr：

δvf=-k8*θf（35）

δvr=-k8*θr（36）

其中，k8为pid回路中的第十三预设值；

e）所述控制模块根据公式（23）-（26）计算四个驱动轮的目标控制速度vfl、vfr、vrl和vrr；

f）所述控制模块将步骤e）计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述驱动装置组；

g）所述驱动装置组根据所述电机转速控制电机，调整所述主动轮、从动轮驱动装置组的方向；

h）进入步骤a）。

优选地，所述第十预设值k7和第十一预设值ω1通过实际测试实验获得。

与现有技术相比较，本发明的有益效果是提供一种惯导重载agv系统及其控制方法，对双差速运动控制模型进行了简化，采用主动轮跟踪目标轨迹，从动轮跟踪主动轮的控制方法，避免惯性导航方式与双差速驱动结合时需考虑复杂车体模型的问题，同时适用于嵌入式平台，实现了惯性导航方式下双差速重载嵌入式agv，解决了传统磁条导航重载agv不适用于地面环境较为恶劣的工厂的难题。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。

图1为本发明一种惯导重载agv系统的agv横截面的侧视图；

图2为本发明一种惯导重载agv系统的agv车底的仰视图；

图3为本发明一种惯导重载agv系统的agv车体横截面的侧视图；

图4为本发明一种惯导重载agv系统的惯性导航标定模式的方法流程图；

图5为本发明一种惯导重载agv系统的惯性导航调整模式的方法流程图；

图6为本发明一种惯导重载agv系统的原地旋转控制模式的方法流程图。

其中：1-驱动装置组，11-前轮驱动装置，111-左前轮驱动装置，112-右前轮驱动装置，113-前轮驱动装置连接机构，12-后轮驱动装置，121-左后轮驱动装置，122-右后轮驱动装置，123-后轮驱动装置连接机构，13-电机；2-控制模块；3-惯性导航模块，31-磁尺传感器组，311-前端磁尺传感器，312-后端磁尺传感器，32-磁钉组，33-陀螺仪；4-agv车体，41-前轮减震系统，42-前轮减震系统导向轴，43-前轮销轴，44-后轮减震系统，45-后轮减震系统导向轴，46-后轮销轴。

具体实施方式

为使本申请的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本申请作进一步详细的说明。

本发明提供一种惯导重载agv系统，如图1、图2和图3所示，所述惯导重载agv系统包括：驱动装置组1、控制模块2、惯性导航模块3和agv车体4；

所述驱动装置组1包括前轮驱动装置组11和后轮驱动装置组12；所述前轮驱动装置组11包括左前轮驱动装置111和右前轮驱动装置112；所述后轮驱动装置组12包括左后轮驱动装置121和右后轮驱动装置122；所述左前轮驱动装置111和所述右前轮驱动装置112安装在前轮驱动装置连接机构113的左右两端；所述前轮驱动装置连接机构113中心对称地水平安装在所述agv车体4前端下方；所述前轮驱动装置连接机构113和所述agv车体4的前轮减震系统41的导向轴42通过前轮销轴43连接；所述左后轮驱动装置121和所述右后轮驱动装置122安装在后轮驱动装置连接机构123的左右两端；所述后轮驱动装置连接机构123中心对称地水平安装在所述agv车体4后端下方；所述后轮驱动装置连接机构123和所述agv车体4的后轮减震系统44的导向轴45通过后轮销轴46连接；所述前轮减震系统41的导向轴42和所述后轮减震系统44的导向轴45均可以上下移动和水平转动；

所述驱动装置的电机13上均安装有速度编码器，用于采集所述驱动装置中的驱动轮的速度信息，并发送到所述控制模块2；所述前轮减震系统41的导向轴42上安装有角度编码器，用于获得前轮驱动装置组11竖直方向与所述agv车体4竖直方向的前轮夹角信息，并发送到所述控制模块2；所述agv车体4竖直方向为所述agv车体4前后的中轴线平行的方向，所述前轮驱动装置组11的竖直方向为垂直于所述前轮驱动装置连接机构113的方向；所述后轮减震系统44的导向轴45上安装有角度编码器，用于获得所述后轮驱动装置组12竖直方向与所述agv车体4竖直方向的后轮夹角信息，并发送到所述控制模块2；所述后轮驱动装置组12的竖直方向为垂直于所述后轮驱动装置连接机构123的方向；所述控制模块2通过所述驱动装置组1为驱动轮设置差速控制所述agv车体4转向；

所述惯性导航模块3包括磁尺传感器组31、n组磁钉组32和陀螺仪33，n≥1；每组磁钉组32包含m个磁钉，m≥2；所述n组磁钉组32用于指示所述惯性导航模块3的目标轨迹；所述磁尺传感器组31包括前端磁尺传感器311和后端磁尺传感器312；所述前端磁尺传感器311安装在所述agv车体4的前端；所述后端磁尺传感器312安装在所述agv车体4的后端；当所述agv车体4前进时，所述前端磁尺传感器311检测计算磁钉位置信号，并发送到所述控制模块2；当所述agv车体4后退时，所述后端磁尺传感器312检测计算磁钉位置信号，并发送到所述控制模块2；所述磁钉位置信号包括当磁尺传感器经过磁钉时，所述磁钉相对磁尺中心的位置；所述陀螺仪33安装在所述agv车体4的原地旋转中心，用于采集陀螺仪信号，所述陀螺仪信号为所述agv车体4相对于所述agv开机上电启动时的实时方位角，并发送到所述控制模块2；所述控制模块2接收所述磁钉位置信号、所述陀螺仪信号、所述驱动装置组1中的车轮组的速度信息、前轮夹角信息和后轮夹角信息，控制所述驱动装置组1带动所述agv车体4沿磁钉组32铺设方向和中心前进；其中，所述目标轨迹为直线目标轨迹；

所述磁尺传感器组31检测到所述磁钉位置信号后将所述磁钉位置信号发送给所述控制模块2，所述控制模块2将所述agv切换到惯性导航标定模式，对所述agv车体4的方向和位置进行标定；在惯性导航标定模式下，所述磁尺传感器组31未检测到所述磁钉位置信号后将未检测到所述磁钉位置信号发送给所述控制模块2，所述控制模块2将所述agv切换到惯性导航调整模式，调整所述agv车体4沿着所述目标轨迹行走。

所述n组磁钉组32中任意相邻的两个磁钉之间的距离大于所述磁尺传感器可检测到单个磁钉位置信号时磁尺与磁钉之间最大距离的两倍。

每组磁钉组32中所述m个磁钉之间的距离是相等的。

其中，m=2。

一种惯导重载agv系统的控制方法，包括惯性导航标定模式标定方法和惯性导航调整模式控制方法；当所述agv前进时，所述前轮驱动装置组11为主动轮驱动装置组，所述后轮驱动装置组12为从动轮驱动装置组；当所述agv后退时，所述后轮驱动装置组12为主动轮驱动装置组，所述前轮驱动装置组11为从动轮驱动装置组；所述主动轮驱动装置组跟踪所述目标轨迹，所述从动轮驱动装置组跟随所述主动轮驱动装置组的方向，使车身方向沿着目标轨迹的方向；

在所述惯性导航标定模式下，使用所述惯性导航标定模式标定方法对所述agv车体4的方向和位置进行标定，如图4所示，所述惯性导航标定模式标定方法包括以下步骤：

1）所述agv向前运动，经过一组磁钉组32中的第i个磁钉的时候，记录当前di值；如果i=1，所述agv继续前进；如果i≥2，进入步骤2）；

其中，di为所述磁尺传感器组31检测到的第i个磁钉相对磁尺中心的位置，磁尺左边方向为负值，磁尺右边方向为正值；

2）根据公式（1）-（2）计算所述agv车体4的方向与所述目标轨迹的方向间的偏差角度δθi：

δθi=arcsin((di-1-di)/li)（1）

其中：

di-1：所述磁尺传感器组31检测到的第i-1个磁钉相对磁尺中心的位置，磁尺左边方向为负值，磁尺右边方向为正值；

li：所述第i个磁钉与所述第i-1个磁钉之间的距离；

δθi：所述agv车体4经过所述第i个磁钉时所述agv车体4的方向相对于所述目标轨迹的方向的偏差角度；

3）判断i是否等于m，如果是，进入步骤4）；如果否，回到步骤1）；

4）根据公式（2）计算δθi的平均值δθ：

δθ=/(m-1)（2）

5）根据公式（3）计算所述目标轨迹的方向在陀螺仪大地坐标系中相对所述陀螺仪33初始方位角的角度θ0；

θ0=θc+δθ（3）

陀螺仪大地坐标系：所述陀螺仪33在大地平面内的旋转坐标系，以所述agv开机上电启动时所述陀螺仪33的初始方位角为0度，逆时针方向为正；

θc：当所述agv车体4经过第m个磁钉时所述陀螺仪33的信号，用于表示所述agv车体4在所述陀螺仪大地坐标系中的实时方向；

6）根据公式（4）计算当所述agv车体4经过第m个磁钉时所述主动轮驱动装置组中心点a到所述目标轨迹的垂直距离δx0，所述目标轨迹前进方向右边为正：

δx0=dm*cosδθ-d*sinδθ（4）

其中：

d：所述主动轮驱动装置组中心点a与检测所述磁钉位置信号的磁尺传感器的垂直距离；

在所述惯性导航调整模式下，使用所述惯性导航调整模式控制方法调整所述agv沿着目标轨迹行走，如图5所示，包括以下步骤：

1)所述控制模块2等待定时器采样信号，若无，则继续等待；若有，则进入步骤2）；

2）所述控制模块2采集所述陀螺仪信号θc、所述主动轮驱动装置组的左右两个驱动轮的实时速度信息vfl和vfr、所述驱动装置组1中的主动轮夹角信息θf和从动轮夹角信息θr；其中，θf为所述主动轮驱动装置组竖直方向与所述agv车体4竖直方向的实时夹角，顺时针方向为正，由所述主动轮驱动装置组上的角度编码器实时测量获得；θr为所述从动轮驱动装置组竖直方向与所述agv车体4竖直方向的实时夹角，顺时针方向为正，由所述从动轮驱动装置组上的角度编码器实时测量获得；

3）所述控制模块2计算所述主动轮驱动装置组的左右两个驱动轮的目标控制速度vfl、vfr，包括如下步骤：

a）所述控制模块2计算|θ0-θc+θf|，如果|θ0-θc+θf|≤第一预设值，则设置所述主动轮驱动装置组中左右两侧驱动轮的转速差控制量δvf=0，进入步骤e）；如果|θ0-θc+θf|＞所述第一预设值，则进入步骤b）；所述第一预设值为所述主动轮驱动装置组竖直方向相对目标轨迹方向的偏移角度阈值；

b）根据公式（5）-（6）计算转速差控制分量δvf1、δvf2：

δvf1=-k1*(θ0-θc)（5）

δvf2=-k2*θf（6）

其中，k1为pid回路中的第二预设值；k2为pid回路中的第三预设值；

c）根据公式（7）-（10）计算转速差控制分量δvf3：

vf=（vfl+vfr）/2（7）

δx=δx0+σδt*sin(θ0-θc+θf)*vf（8）

θδx=arcsin(-k3*δx)（9）

δvf3=k1*θδx（10）

其中，k3为pid回路中的第四预设值，δx为在每一段计算周期δt内所述主动轮驱动装置组中心点a在垂直于所述目标轨迹方向上的位置的积分，vf为所述主动轮驱动装置组中心点a的实时速度，vfl、vfr为所述主动轮驱动装置组的左右两个驱动轮的实时速度，由所述主动轮驱动装置组上的速度编码器实时测量获得；在本发明中，k3实际取值为1/200，δx≤200mm；

d）根据公式（11）计算所述主动轮驱动装置组中左右两个驱动轮的转速差控制量δvf；

δvf=δvf1+δvf2+δvf3（11）

e）根据公式（12）计算所述主动轮驱动装置组中心点a的目标控制速度vf，所述agv车体4运动方向为正：

vf=v/cosθf（12）

其中，v为所述agv沿所述目标轨迹方向的预设的运动速度；

f）根据公式（13）-（14）计算所述主动轮驱动装置组的左右两个驱动轮的目标控制速度vfl、vfr：

vfl=vf+δvf（13）

vfr=vf-δvf（14）

4）所述控制模块2计算所述从动轮驱动装置组的左右两个驱动轮的目标控制速度vrl和vrr，包括如下步骤：

a）所述控制模块2计算|θ0-θc+θr|，如果|θ0-θc+θr|≤第五预设值，则设置所述从动轮驱动装置组中左右两侧驱动轮的转速差控制量δvr=0，进入步骤c）；如果|θ0-θc+θr|＞所述第五预设值，则进入步骤b）；所述第五预设值为所述从动轮驱动装置组竖直方向相对目标轨迹方向的偏移角度阈值；

b）根据公式（15）-（17）计算所述从动轮驱动装置组中左右两侧驱动轮的转速差控制量δvr：

δvr1=-k4*(θ0-θc)（15）

δvr2=-k5*θr（16）

δvr=δvr1+δvr2（17）

c）根据公式（18）计算所述从动轮驱动装置组中心点b的目标控制速度vr，所述agv车体4运动方向为正：

vr=v/cosθr（18）

d）根据公式（19）-（20）计算所述从动轮驱动装置组的左右两个驱动轮的目标控制速度vrl和vrr：

vrl=vr+δvr（19）

vrr=vr-δvr（20）

5）所述控制模块2将步骤3）和4）中计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述主动轮、从动轮驱动装置组；

6）所述主动轮、从动轮驱动装置组根据所述电机转速控制电机13，调整所述agv车体4的车身方向和位置；

7）进入步骤1）。

所述第一预设值与所述第五预设值相等。

其中，k1=k2，k4=k5。

其中，k1=k4，k2=k5。

所述惯导重载agv系统还包括原地旋转控制模式，如图6所示，所述原地旋转控制模式包括如下步骤：

1）所述控制模块2接收到调度系统发送的原地旋转命令；

2）所述控制模块2控制所述agv停车；

3）所述控制模块2调整所述主动轮、从动轮驱动装置组的方向，包括如下步骤：

a)所述控制模块2等待定时器采样信号，若无，则继续等待；若有，则进入步骤b）；

b）所述控制模块2采集所述驱动装置组1中的主动轮夹角信息θf和从动轮夹角信息θr；

c）所述控制模块2计算|θf-90°|，如果|θf-90°|≤第六预设值，所述第六预设值为所述主动轮驱动装置组竖直方向相对所述agv车体4竖直方向的偏移角度阈值，进行步骤4）；如果|θf-90°|＞所述第六预设值，进行下一步；在本发明中，所述第六预设值实际取值为3.2°；

d）根据公式（21）和（22）计算δvf和δvr：

δvf=-k6*（θf-90°）（21）

δvr=-k6*（θr-90°）（22）

其中，k6为pid回路中的第七预设值；

e）根据公式（23）-（26）计算所述驱动装置组1的四个驱动轮的目标控制速度vfl、vfr、vrl和vrr：

vfl=δvf（23）

vfr=-δvf（24）

vrl=δvr（25）

vrr=-δvr（26）

f）所述控制模块2将步骤e）计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述驱动装置组1；

g）所述驱动装置组1根据所述电机转速控制电机13，调整所述主动轮、从动轮驱动装置组的方向；

h）进入步骤3）的步骤a）；

4）所述控制模块2匀速旋转所述agv车体4，包括如下步骤：

a)所述控制模块2等待定时器采样信号，若无，则继续等待；若有，则进入步骤b）；

b)所述控制模块2采集所述陀螺仪信号并记录为θc旋转前，用于表示所述agv车体4开始旋转前在所述陀螺仪大地坐标系中的方位角；

c)所述控制模块2根据公式（27）计算所述agv车体4在所述陀螺仪大地坐标系中要旋转到的目标角度θtarget：

θtarget=θc旋转前+θturn（27）

其中，θturn为预设的相对旋转角度；

d）所述控制模块2等待定时器采样信号，若无，则继续等待；若有，则进入步骤e）；

e）所述控制模块2采集当前时刻所述陀螺仪信号θc；

f）所述控制模块2根据公式（28）计算所述agv车体4在所述陀螺仪大地坐标系中的当前角度与所述要旋转到的目标角度的差值δθremain：

δθremain=θc-θtarget（28）

g）所述控制模块2对步骤f）计算的δθremain进行判断，如果-sign(θturn)*δθremain＜δθturn，进行步骤4）；如果-sign(θturn)*δθremain≥δθturn，进行下一步；其中，δθturn为第七预设值；在本发明中，所述第七预设值实际取值为5°；

h）根据公式（21）-（22）计算δvf和δvr；根据公式（29）~（33）计算四个驱动轮的目标控制速度vfl、vfr、vrl和vrr：

vfl=ωr1+∆vf（29）

vfr=ωr2-∆vf（30）

vrl=-ωr2+∆vr（31）

vrr=-ωr1-∆vr（32）

ω=ω0（33）

其中：

ω：所述agv车体4的目标旋转角速度，顺时针方向为正；

ω0：所述agv车体4的预设旋转角速度，ω0为第八预设值，顺时针方向为正；

r1：所述主动轮驱动装置组竖直方向与所述agv车体4竖直方向垂直时外侧主动轮到所述agv车体4中心的距离；

r2：所述主动轮驱动装置组竖直方向与所述agv车体4竖直方向垂直时内侧主动轮到所述agv车体4中心的距离；

i）所述控制模块2将步骤h）计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述驱动装置组1；

j）所述驱动装置组1根据所述电机转速控制电机13，调整所述agv车体4的车身方向；

k）进入步骤a）；

5）所述控制模块2控制所述agv车体4减速旋转直至停止，包括如下步骤：

a)所述控制模块2等待定时器采样信号，若无，则继续等待；若有，则进入步骤b）；

b）所述控制模块2采集当前时刻的所述陀螺仪信号θc；

c）所述控制模块2根据公式（28）计算δθremain；

d）所述控制模块2判断是否δθremain≤第九预设值，如果是，则停止所述agv车体4，旋转结束，进入步骤5）；如果否，则进入步骤e）；

e）所述控制模块2根据公式（34）计算目标旋转角速度ω值：

ω=ω1+k7*δθremain(34)

其中，k7为第十预设值，ω1为第十一预设值；

f）所述控制模块2根据公式（29）~（32）和步骤e）中计算的ω计算四个驱动轮的目标控制速度vfl、vfr、vrl和vrr；

g）所述控制模块2将步骤f）计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述驱动装置组1；

h）所述驱动装置组1根据所述电机转速控制电机13，调整所述agv车体4的车身方向；

i）进入步骤a）；

6）所述控制模块2调整所述主动轮、从动轮驱动装置组的方向，包括如下步骤：

a)所述控制模块2等待定时器采样信号，若无，则继续等待；若有，则进入步骤b）；

b）所述控制模块2采集所述驱动装置组1中的主动轮夹角信息θf和从动轮夹角信息θr；

c）所述控制模块2计算|θf|，如果|θf|≤第十二预设值，则原地旋转完成，退出原地旋转控制模式，所述第十二预设值为主动轮驱动装置组竖直方向相对所述agv车体4竖直方向的偏移角度阈值；如果|θf|＞所述第十二预设值，进入步骤b）；在本发明中，所述第十二预设值实际取值为3.2°；

d）所述控制模块2根据公式（35）和（36）计算δvf和δvr：

δvf=-k8*θf（35）

δvr=-k8*θr（36）

其中，k8为pid回路中的第十三预设值；

e）所述控制模块2根据公式（23）-（26）计算四个驱动轮的目标控制速度vfl、vfr、vrl和vrr；

f）所述控制模块2将步骤e）计算的所述驱动轮的目标控制速度转换成电机转速，并且发送给所述驱动装置组1；

g）所述驱动装置组1根据所述电机转速控制电机13，调整所述主动轮、从动轮驱动装置组的方向；

h）进入步骤a）。

所述第十预设值k7和第十一预设值ω1通过实际测试实验获得。

与现有技术相比，本发明提供的一种惯导重载agv系统及其控制方法，对双差速运动控制模型进行了简化，采用主动轮跟踪目标轨迹，从动轮跟踪主动轮的控制方法，避免惯性导航方式与双差速驱动结合时需考虑复杂车体模型的问题，同时适用于嵌入式平台的问题，实现了惯性导航方式下双差速重载嵌入式agv，解决了传统磁条导航重载agv不适用于地面环境较为恶劣的工厂的难题。