CN109460038A - 一种惯导重载agv系统及其控制方法 - Google Patents

一种惯导重载agv系统及其控制方法 Download PDF

Info

Publication number
CN109460038A
CN109460038A CN201811598883.5A CN201811598883A CN109460038A CN 109460038 A CN109460038 A CN 109460038A CN 201811598883 A CN201811598883 A CN 201811598883A CN 109460038 A CN109460038 A CN 109460038A
Authority
CN
China
Prior art keywords
group
car body
control module
agv
wheel
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201811598883.5A
Other languages
English (en)
Other versions
CN109460038B (zh
Inventor
石飞
李星
邓杨
黄怡
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
NANJING JINGYAO INTELLIGENT SCIENCE & TECHNOLOGY Co Ltd
Original Assignee
NANJING JINGYAO INTELLIGENT SCIENCE & TECHNOLOGY Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by NANJING JINGYAO INTELLIGENT SCIENCE & TECHNOLOGY Co Ltd filed Critical NANJING JINGYAO INTELLIGENT SCIENCE & TECHNOLOGY Co Ltd
Priority to CN201811598883.5A priority Critical patent/CN109460038B/zh
Publication of CN109460038A publication Critical patent/CN109460038A/zh
Application granted granted Critical
Publication of CN109460038B publication Critical patent/CN109460038B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种惯导重载AGV系统及其控制方法，所述惯导重载AGV系统包括：驱动装置组、控制模块、惯性导航模块和AGV车体；所述惯性导航模块用于全局导航，所述控制模块用于对所述AGV车体的方向和位置进行标定和通过控制所述驱动装置组调整所述AGV车体沿着目标轨迹的方向行走。本发明解决的技术问题在于提供一种惯导重载AGV系统及其控制方法，对双差速运动控制模型进行了简化，采用主动轮跟踪目标轨迹，从动轮跟踪主动轮的控制方法，避免惯性导航方式与双差速驱动结合时需考虑复杂车体模型的问题，同时可适用于嵌入式平台。

Description

一种惯导重载AGV系统及其控制方法
技术领域
本发明涉及到AGV技术领域，特指一种惯导重载AGV系统及其控制方法。
背景技术
AGV(Automated Guide Vehicle，自动导引运输车)是指装备有电磁或光学等自动导引装置的运输车，无需人工参与就可以随着生产工艺流程的调整而及时调整相应的运输路线，由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不适用于地面环境较为恶劣的工厂的难题。

Claims (10)

1.一种惯导重载AGV系统，其特征在于，所述惯导重载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）沿着所述目标轨迹行走。
2.根据权利要求1所述的一种惯导重载AGV系统，其特征在于，所述N组磁钉组（32）中任意相邻的两个磁钉之间的距离大于所述磁尺传感器可检测到单个磁钉位置信号时磁尺与磁钉之间最大距离的两倍。
3.根据权利要求1所述的一种惯导重载AGV系统，其特征在于，每组磁钉组（32）中所述M个磁钉之间的距离是相等的。
4.根据权利要求1所述的一种惯导重载AGV系统，其特征在于，M=2。
5.一种根据权利要求1所述的一种惯导重载AGV系统的控制方法，其特征在于，包括惯性导航标定模式标定方法和惯性导航调整模式控制方法；当所述AGV前进时，所述前轮驱动装置组（11）为主动轮驱动装置组，所述后轮驱动装置组（12）为从动轮驱动装置组；当所述AGV后退时，所述后轮驱动装置组（12）为主动轮驱动装置组，所述前轮驱动装置组（11）为从动轮驱动装置组；所述主动轮驱动装置组跟踪所述目标轨迹，所述从动轮驱动装置组跟随所述主动轮驱动装置组的方向；
在所述惯性导航标定模式下，使用所述惯性导航标定模式标定方法对所述AGV车体（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沿着目标轨迹行走，包括以下步骤：
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为所述主动轮驱动装置组的左右两个驱动轮的实时速度，由所述主动轮驱动装置组上的速度编码器实时测量获得；
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）。
6.根据权利要求5所述的一种惯导重载AGV系统的控制方法，其特征在于，所述第一预设值与所述第五预设值相等。
7.根据权利要求5所述的一种惯导重载AGV系统的控制方法，其特征在于，k1=k2， k4=k5。
8.根据权利要求5所述的一种惯导重载AGV系统的控制方法，其特征在于，k1=k4， k2=k5。
9.根据权利要求1所述的一种惯导重载AGV系统，其特征在于，还包括原地旋转控制模式，所述原地旋转控制模式包括如下步骤：
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°|＞所述第六预设值，进行下一步：
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为第七预设值；
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）；
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）。
10.根据权利要求9所述的一种惯导重载AGV系统，其特征在于，所述第十预设值k7和第十一预设值ω1通过实际测试实验获得。
CN201811598883.5A 2018-12-26 2018-12-26 一种惯导重载agv系统及其控制方法 Active CN109460038B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811598883.5A CN109460038B (zh) 2018-12-26 2018-12-26 一种惯导重载agv系统及其控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811598883.5A CN109460038B (zh) 2018-12-26 2018-12-26 一种惯导重载agv系统及其控制方法

Publications (2)

Publication Number Publication Date
CN109460038A true CN109460038A (zh) 2019-03-12
CN109460038B CN109460038B (zh) 2024-05-21

Family

ID=65614990

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811598883.5A Active CN109460038B (zh) 2018-12-26 2018-12-26 一种惯导重载agv系统及其控制方法

Country Status (1)

Country Link
CN (1) CN109460038B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109976372A (zh) * 2019-04-24 2019-07-05 重庆大学 一种基于磁导航的四驱agv姿态调整运动模型
CN113625713A (zh) * 2021-08-11 2021-11-09 北京京东振世信息技术有限公司 自动引导运输车的控制方法和装置

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101885351A (zh) * 2010-07-15 2010-11-17 南京航空航天大学 可分式差速驱动装置及其全方位移动自动导引车
KR20120042394A (ko) * 2010-10-25 2012-05-03 부산대학교 산학협력단 주행 장치를 위한 위치측정 시스템 및 방법
CN102564420A (zh) * 2011-12-24 2012-07-11 浙江大学 适用于捷联惯性导航系统的惯性传感器级旋转调制方法
CN105180932A (zh) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 一种适合仓储agv的惯性导航方法
CN106314594A (zh) * 2016-08-26 2017-01-11 南京理工大学 一种用于现代物流行业的仓储搬运机器人
CN205950750U (zh) * 2016-08-18 2017-02-15 广西电网有限责任公司北海供电局 基于惯性导航的变电站巡检机器人导航控制系统
CN206115277U (zh) * 2016-10-21 2017-04-19 北京京东尚科信息技术有限公司 Agv运输车
CN107272008A (zh) * 2017-06-14 2017-10-20 上海大学 一种带惯性补偿的agv激光导航系统
WO2018072712A1 (zh) * 2016-10-21 2018-04-26 北京京东尚科信息技术有限公司 Agv运输车及其控制方法
CN108583274A (zh) * 2018-03-22 2018-09-28 苏州极客嘉智能科技有限公司 一种多角度移动功能的agv差速驱动机构
CN208181253U (zh) * 2018-05-24 2018-12-04 合肥井松自动化科技有限公司 一种全方位双差速视觉导航智能agv车体结构
CN108958269A (zh) * 2018-10-09 2018-12-07 南京景曜智能科技有限公司 一种惯导磁条混合导航agv系统

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101885351A (zh) * 2010-07-15 2010-11-17 南京航空航天大学 可分式差速驱动装置及其全方位移动自动导引车
KR20120042394A (ko) * 2010-10-25 2012-05-03 부산대학교 산학협력단 주행 장치를 위한 위치측정 시스템 및 방법
CN102564420A (zh) * 2011-12-24 2012-07-11 浙江大学 适用于捷联惯性导航系统的惯性传感器级旋转调制方法
CN105180932A (zh) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 一种适合仓储agv的惯性导航方法
CN205950750U (zh) * 2016-08-18 2017-02-15 广西电网有限责任公司北海供电局 基于惯性导航的变电站巡检机器人导航控制系统
CN106314594A (zh) * 2016-08-26 2017-01-11 南京理工大学 一种用于现代物流行业的仓储搬运机器人
CN206115277U (zh) * 2016-10-21 2017-04-19 北京京东尚科信息技术有限公司 Agv运输车
WO2018072712A1 (zh) * 2016-10-21 2018-04-26 北京京东尚科信息技术有限公司 Agv运输车及其控制方法
CN107272008A (zh) * 2017-06-14 2017-10-20 上海大学 一种带惯性补偿的agv激光导航系统
CN108583274A (zh) * 2018-03-22 2018-09-28 苏州极客嘉智能科技有限公司 一种多角度移动功能的agv差速驱动机构
CN208181253U (zh) * 2018-05-24 2018-12-04 合肥井松自动化科技有限公司 一种全方位双差速视觉导航智能agv车体结构
CN108958269A (zh) * 2018-10-09 2018-12-07 南京景曜智能科技有限公司 一种惯导磁条混合导航agv系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
吕修文: "《上海交通大学硕士学位论文》", 31 December 2017 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109976372A (zh) * 2019-04-24 2019-07-05 重庆大学 一种基于磁导航的四驱agv姿态调整运动模型
CN113625713A (zh) * 2021-08-11 2021-11-09 北京京东振世信息技术有限公司 自动引导运输车的控制方法和装置
CN113625713B (zh) * 2021-08-11 2024-04-16 北京京东振世信息技术有限公司 自动引导运输车的控制方法和装置

Also Published As

Publication number Publication date
CN109460038B (zh) 2024-05-21

Similar Documents

Publication Publication Date Title
CN101486360B (zh) 带有导臂的履带式移动机器人的攀爬楼梯控制方法
CN106020200B (zh) 采用轮毂电机驱动的agv小车及路径规划方法
CN106950964B (zh) 无人电动大学生方程式赛车及其控制方法
CN105172793B (zh) 自动驾驶汽车的位姿估算方法
CN110001840B (zh) 一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法
CN105180933B (zh) 基于直行路口检测的移动机器人航迹推算修正系统及方法
CN105843229A (zh) 无人驾驶智能小车及控制方法
CN105501288B (zh) 转向助力控制装置和转向助力控制方法
CN107179091B (zh) 一种agv行走视觉定位误差修正方法
US20190092464A1 (en) Hybrid unmanned vehicles and related methods
CN111776243B (zh) 一种登机桥的控制方法及装置
CN109460038A (zh) 一种惯导重载agv系统及其控制方法
CN111026112B (zh) 一种轮式机器人沿斜坡中线自动行走的控制系统
CN106946049A (zh) 集装箱码头流动设备自动化行走方法
CN205294284U (zh) 集装箱码头流动设备自动化行走系统
CN112238451A (zh) 一种打滑检测方法及装置
CN109552417A (zh) 一种驱动轮角度零偏的校准方法及系统
CN110077392A (zh) 一种自动泊车定位系统的航迹推算方法
CN110320906A (zh) 一种基于麦克纳姆轮的四驱agv小车差速直线行驶姿态调整方法
CN105241449A (zh) 平行架构下巡检机器人视觉导航方法与系统
CN110244718A (zh) 一种可自动避障的巡视智能小车
WO2006087542A1 (en) Vehicle location
CN113405458B (zh) 一种截割头精准坐标检测系统及检测方法
CN207540557U (zh) 一种用于agv小车的短时精确定位的装置
CN108958269A (zh) 一种惯导磁条混合导航agv系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant