发明内容
发明目的:针对现有技术中存在的问题,本发明提供一种可自动避障的巡视智能小车。
技术方案:本发明提供了一种可自动避障的巡视智能小车,包括小车本体,小车本体上端固定有一核心板,巡视智能小车车头位置还固定有超声波模组与电磁模组,所述超声波模组、电磁模组与核心板信号连接,所述小车本体车轮上固定有驱动电机,所述驱动电机上固定有光栅测速器,且所述驱动电机与光栅测速器均与所述核心板电性连接;
所述核心板通过预设程序设计巡视智能小车避障运动模型,并控制其按所述预设程序设置好的轨迹行进,所述预设程序的运动控制算法包括如下步骤:
步骤1:根据巡视智能小车结构外观,结合坐标系建立方法,建立巡视智能小车直行局部坐标系与转向全局坐标系;
步骤2:根据巡视智能小车的行进路线,结合其结构外观,画出其转向运动轨迹,得到理想位姿方程,所述理想位姿方程为:
其中,Δα为航向角变化率,vax为x轴方向参考点速度,vay为y轴方向重心速度,α为转向时航向角,r左侧前后轮距转向圆心点P的距离,R巡视智能小车右侧前后轮距转向圆心点P的距离,β为通过的圆弧弧度,L为重心与参考点之间的距离,ωl为左侧车轮左转角速度,ωr为右侧车轮左转角速度。
步骤3:使用离散求和的方法得到智能小车理想情况下的下一时刻运动参数;
步骤4:根据巡视智能小车期望参数与实际参数的误差,进行经典的PID与增量式PID算法分析比较,基于增量式PID改进算法对理想位姿方程算法修正,消除误差。
优选的,所述步骤4中基于增量式PID改进算法,算法修正后的位姿方程为:
其中,n为t时刻的采样间隔,ωl(n)、ωr(n)、α(n)为n时刻左侧车轮角速度、右侧车轮角速度及航向角,vax(n+1)、vay(n+1)、Δα(n+1)为n+1时刻位姿,eL(n)、eR(n)、e(n)分别为n时刻左侧车轮反馈增量、右侧车轮反馈增量与车轮间内环反馈增量。
优选的,所述核心板为Cortex-A8核心板。
优选的,所述超声波模组包括3个超声波传感器,位于核心板前端即智能小车车头位置,分别位于车头正前方位置以及左右两侧45度角位置,用于监测三个方向的障碍物距离。
优选的,超声波传感器型号为HC-SR04+。
优选的,所述电磁模组包括多个电磁传感器,所述电磁传感器位于智能小车核心板前端车头位置,用于监测巡视智能小车行进路线是否偏出轨迹。
优选的,所述电磁传感器为霍尔电磁传感器,其型号为MRS201。
优选的,所述驱动组件包括固定于小车本体车轮上的驱动电机,所述驱动电机的输出轴与车轮驱动连接。
有益效果:
1.该可自动避障的巡视智能小车,通过在智能小车的Cortex-A8核心板上集成超声波传感器、电磁传感器,并在核心板上预先设定程序,通过核心板接收到的传感器信息,并通过预先设定的程序控制智能小车按照既定的运动轨迹运行,实现避障的功能。
2.该可自动避障的巡视智能小车,先建立了理想的运动模型便于分析,通过建立坐标系推到出巡视智能小车的理想位姿方程与角速度和航向角变化率的关系,后通过离散求和的方法得到理想情况下的下一时刻角速度与航向角变化率,再通过经典PID与增量式PID算法分析比较,采用基于增量式PID改进算法进行消除误差,获得消除误差后的角速度再回代回理想位姿方程,得到更精准的位姿方程,使它可以在无需人为干预下参照既定运行轨迹运行,确保智能小车运行的稳定性。
3.该可自动避障的巡视智能小车在车头前端使用3个超声波传感器,并且位于车头位置以及车头左哟45度角位置,可以多方位监测小车前方障碍物的距离。
4.该可自动避障的巡视智能小车在车头位置使用多个电磁传感器,并且并列固定于智能小车车头位置,可实时监测巡视智能小车行进路线是否偏出轨迹。
5.该可自动避障的巡视智能小车在小车车轮的驱动电机上固定光栅测速器,并与核心板电性连接,可以方便测出小车两侧车轮的线速度。
附图说明
图1为本发明提出的一种可自动避障的巡视智能小车的结构示意图;
图2为本发明提出的一种可自动避障的巡视智能小车的俯视图;
图3为本发明提出的一种可自动避障的巡视智能小车的运动控制算法的系统流程图;
图4为本发明提出的一种可自动避障的巡视智能小车的直行局部坐标系;
图5为本发明提出的一种可自动避障的巡视智能小车的转向全局坐标系;
图6为本发明提出的一种可自动避障的巡视智能小车的转向运动轨迹;
图7为本发明提出的一种可自动避障的巡视智能小车的增量式PID算法框图;
其中,1-小车本体;101-底板;102-核心板;103车头板;104-支架;105-车轮;2-超声波模组;3-电磁模组。
具体实施方式
下面结合附图对本发明进行详细的介绍。
参照图1、图2、图3,该巡视智能小车在小车本体1的底板101上端通过支架104固定有一个核心板102,本申请中的支架104为4根支撑柱,分为固定于底板101的四个角上,将核心板102的四个角通过焊接或者紧固件方式固定于4根支撑柱上,本申请中采用了Cortex-A8系列核心板中的FET210核心板作为主控核心板,负责控制小车本体1上车轮105上的驱动电机完成巡视智能小车的行进与转向等动作,驱动电机的输出轴与车轮105连接,将驱动电机的动力输出传输给车轮105,在驱动电机上还固定有一个光栅测速器,本申请中的光栅测速器就是普通的光栅测速传感器,在深圳宇铭科技有限公司购得的光栅测速码盘,光栅测速器用来读取单位时间内电机转动圈数,驱动电机与光栅测速器与核心板102电性连接,核心板获取光栅测速器的数据,得到小车本体1车轮105的线速度,根据线速度与预先设定好的运行轨迹的半径,可以得到小车本体1车轮105的角速度。在巡视智能小车内部还固定有电源模块,电源模块与驱动电机、光栅测速器等线路连接,用来为巡视智能小车提供电力。在小车本体1的车头位置即核心板前端,通过支架104(即智能小车前端的两根支撑柱)固定有一个车头板103,车头板103上通过焊接或者紧固件方式固定有一个电磁模组3和一个超声波模组2,巡视智能小车的核心板102与电磁模组3和超声波模组2信号连接,核心板102通过电磁模组3、超声波模组2以及光栅测速器获取外部信息,判断出巡视智能小车现在的行进状态,在核心板102中预先设定了程序,通过核心板102中预先设定的程序对巡视智能小车两侧车轮角速度参数进行调节,以完成巡视智能小车的运动控制与避障功能。
所述超声波模组2包括3个超声波传感器,超声波传感器型号为HC-SR04+,3个超声波传感器固定在核心板前端的车头板103上,3个超声波传感器分别位于车头板的正前方位置以及左右两侧45度角位置,用于监测三个方向的障碍物距离。
电磁模组3包括多个电磁传感器,在本实施例中,使用5个电磁传感器,电磁传感器为霍尔电磁传感器,型号为MRS201,5个电磁传感器位于智能小车核心板102前端的车头板103上,用于监测巡视智能小车行进路线是否偏出。
核心板102上预设程序设计智能小车避障运动模型,并控制智能小车按所述预设程序设置好的路线行进,运动控制算法的系统流程图参见附图3。
巡视智能小车避障工作原理:
1、首先打开车轮的驱动电机、光栅测速器、超声波传感器以及电磁传感器;
2、核心板102获取超声波传感器数据,判断前方是否有障碍物,如果没有障碍物,核心板102获取电磁传感器数据,判断是否偏离轨迹,未偏离轨迹的话,小车直行,继续进入步骤2;如果轨迹已有偏离,判断其是否向左偏离,若已向左偏离,核心板控制小车驱动电机驱动车轮向右转动,直到没有偏离轨迹,此时重新步骤2;如果轨迹未向左偏离,则意味着其向右偏离了,核心板控制小车驱动电机驱动车轮向左转,直到没有偏离轨迹,此时重新步骤2;
3、在前方有障碍物的情况下,判断左侧是否有障碍物,无障碍物的情况下,核心板102控制小车驱动电机驱动车轮向左转,按照既定轨迹运行;
4、左侧有障碍物的情况下,判断右侧是否有障碍物,无障碍物的情况下,核心板102控制小车驱动电机驱动车轮向右转,按照既定轨迹运行;
5、右侧也有障碍物的情况下,则核心板102控制驱动电机停止转动,小车停止前进。
针对上诉工作原理,核心板102上所预设程序的运动控制算法包括如下步骤:
步骤1,根据智能小车结构外观,结合坐标系建立方法,建立巡视智能小车局部坐标系与巡视智能小车转向全局坐标系。
参照图4、图5,为该巡视智能小车的直行局部坐标系与转向全局坐标系,其具体内容为:
vgx=vg·sinα (1)
vgy=vg·cosα (2)
vax=vgx-Δα·L·sinα (3)
vay=vgy-Δα·L·cosα (4)
其中,G为智能小车重心,A为智能小车前进参考点,L为重心与参考点之间的距离,点P为预先设定好的转向轨迹的转向圆心,α为转向时航向角,Δα为航向角变化率。r1、r4为左侧前后轮距转向圆心点P的距离,r2、r3为巡视智能小车右侧前后轮距转向圆心点P的距离,其中r1=r4=r,r2=r3=R,vg为转向时重心速度,vgx为x轴方向重心速度,vgy为y轴方向重心速度,va为转向时参考点速度,vax为x轴方向参考点速度,vay为y轴方向重心速度。
步骤2,根据巡视智能小车的行进路线,结合巡视智能小车的结构外观,画出巡视智能小车遇到障碍物的情况下转向运动轨迹,得到巡视智能小车理想位姿方程。
参照图6,为该巡视智能小车转向运动轨迹,结合巡视智能小车的结构外观,其中,s1为右轮位移,s2为左轮位移,vl为左侧车轮左转线速度,通过光栅测速器检测得到,ωl为左侧车轮左转角速度,角速度等于线速度除以运动半径,vr为右侧车轮左转线速度、ωr为右侧车轮左转角速度:
s1=s2+Δα·t·β·R (5)
其中:s1=vr·t,s2=vl·t,vl=ωl·r,vr=ωr·R(6)
将式(6)带入到式(5)中,可以得到:
由此可以得出vg,vr,vl之间的关系:
结合上式(1)(2)(3)(4)(7)(8),可以得到:
由此可以得到参考点A的理想状态下的位姿方程为:
步骤3,使用离散求和的方法得到巡视智能小车下一时刻的角速度与航向角变化率。
步骤4,根据智能小车期望参数与实际参数的误差,进行经典的PID与增量式PID算法分析比较,基于增量式PID改进算法对理想位姿方程进行算法修正,消除误差。
参照图7,为该巡视智能小车的增量式PID算法框图,其中巡视智能小车角速度与航向角变化率为实现运动控制的关键,图7中增量式PID算法框图中的参数为:
vld为左侧车轮期望速度,vld=ωl·r
vrd为右侧车轮期望速度,vrd=ωr·R
vlr为左侧车轮实际速度,vlr=(ωl+eL-e)·r
vrr为右侧车轮实际速度,vrr=(ωl+eR-e)·R
D1为左右轮期望速度之差,D1=vld-vrd
D2为左右轮实际速度之差,D2=vlr-vrr
先使用离散求和的方法得到下一时刻的各项参数,通过增量式PID算法减小巡视智能小车运动过程中的误差,其具体内容为:
ωr(n+1)=ωr(n)+vax·tn (14)
ωl(n+1)=ωl(n)+vay·tn (15)
α(n+1)=α(n)+Δα·tn (16)
其中,n为t时刻的采样间隔,ωl(n+1)、ωr(n+1)、α(n+1)为n+1时刻两边车轮角速度及航向角,ωl(n)、ωr(n)、α(n)为n时刻两边车轮角速度及航向角,vax(n)、vay(n)、Δα(n)为n时刻位姿。
ωl(n+1)=ωl(n)+eL(n)-e(n) (18)
ωr(n+1)=ωr(n)+eR(n)-e(n) (19)
其中,ω
l(n+1)、ω
r(n+1)为n+1时刻两边车轮角速度,ω
l(n)、ω
r(n)、e
L(n)、e
R(n)、e(n)分别为n时刻两边车轮角速度、反馈增量与车轮间内环反馈增量,式(17)中Δu(k)为经过增量式PID算法调节修正后输出的增量,k
p为比例系数,T
i为积分时间常数,T
d为微分时间常数[17],
为积分系数,k
d=k
p·T
d为微分系数。
将上式(18)(19)中n+1时刻两边车轮角速度代入式(13)中就可以得到巡视智能小车在算法修正后的位姿方程为:
式20中,R、r、β是由核心板102将超声波传感器以及电磁传感器采集的数据而推出的,这三个数据为已知数据,是核心板预设程序预设的让巡视智能小车走的轨迹参数值,L为巡视智能小车到参考点A之间的距离,也是已知数值。
ωl、ωr、α这三个参数是核心板102预设程序用来调试的三个参数,改变这个三个参数的值可以改变vax、vay和Δα的值,得到vax、vay和Δα可以推算出一个运动轨迹,当这个推算出的轨迹的参数与之前R、r、β这三个参数一致时,就表明此时的ωl、ωr、α值是可以控制小车走在预设轨迹上的,如果不一致的话就说明此时的ωl、ωr、α参数控制小车走过的轨迹不符合预设轨迹,经过核心板102预设程序运动控制算法软件调试会将参数与预计轨迹一一对应过后,核心板102控制小车按既定参数的轨迹运行。
上述实施方式只为说明本发明的技术构思及特点,其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施,并不能以此限制本发明的保护范围。凡根据本发明精神实质所做的等效变换或修饰,都应涵盖在本发明的保护范围之内。