ROS+GAZEBO田径机器人的仿真研究
仿真turtlebot沿跑道运动
1 视觉获取跑道斜率
通过调用图像处理函数,获取视觉图像里的跑道斜率
2目标点的坐标获取
经过视觉图像处理后,可以得到各线的斜率,大致分三种情况,设已知跑道宽为d
1,小车偏向左侧过多,导致小车右侧跑道线无法识别
此时处理方式如下:
获取的所有线条里找斜率为负值并最小的线条k1,转换为小车坐标系里小车预行驶路线斜率为其取反的值-k1,并设定小车目的坐标为(-d/2k1,-d/2)
2,小车偏向右侧过多,导致小车左侧跑道线无法识别
此时处理方式如下:
获取的所有线条里找斜率为正值并最大的线条k2,转换为小车坐标系里小车预行驶路线斜率为其取反的值-k2,并设定小车目的坐标为(d/2k1,d/2)
3,小车偏向不大,可以识别出所在跑道的左右两线及其他跑道线,如图所示
获取的所有线条里找斜率为负值并最小的线条k1作为小车左侧跑道线,斜率为正直最大的为k2作为小车右侧跑道线,如图所示:
2.1确定小车在图像上位置
由于摄像头位置处于小车上方,所得图片实际小车大于像素点480,接下来我们通过采样某条线上小车图片确定小车位置。
此时通过图像处理程序得到跑道线参数如下
[array([ -1.72405463, 741.61780976]), array([-3.23841272e-03, 3.18958992e+02]), array([ 1.72216076, -104.75660265])]
旋转一个角度,再次得到一组数据
此时通过图像处理程序得到跑道线参数如下
[array([ -4.53150898, 1841.78397193]), array([ -2.39582895, 1315.99880262])]
旋转一个角度,再次得到一组数据
这个位置图像处理无线检出来,小车距离图像点至少0.5m
通过多跑道很难判断具体线对应跑道,采取单线方式确定相机位置
此时通过图像处理程序得到跑道线参数如下
[array([-4.03952074e-02, 3.38940200e+02])]
旋转一个角度,再次得到一组数据
此时通过图像处理程序得到跑道线参数如下
[array([-2.03251762e-01, 7.02310567e+02])]
旋转一个角度,再次得到一组数据
此时通过图像处理程序得到跑道线参数如下
[array([-1.29837145e-01, 5.20188130e+02])]
旋转一个角度,再次得到一组数据
此时通过图像处理程序得到跑道线参数如下
[array([ 0.17367078, 101.51217688])]
2.2估算小车在图像处理坐标中的位置
把小车作为原点,跑道为宽dm,则坐标变换为如下:
此时小车左侧和右侧线分别为(图像识别坐标)
y = k1x+ b1(左侧)
y= k2 x+ b2(右侧)
为了取两条跑道线中间点作为目标点,我们从小车原点分别画斜率为-k1和-k2的两条线,这样在坐标系我们获得一个菱形,取菱形中点z作为小车目标点,如下图所示:
通过已知条件,重合两个坐标系,得到如下图形:
预设图像坐标系中点d1和d2的坐标为(xd1,yd1)和(xd2,yd2),该图像是640480的分辨率,因此可以获得d1点和d2点在图像坐标系的坐标如下:
((480-b1)/k1,480)和((480-b2)/k2,480)
通过坐标变换,得到在小车坐标系中d1和d2的值:
d1= (320-xd1)d/(xd2-xd1)
d2= (xd2-320)d/(xd2-xd1)
变换后的左线右线方程如下:
y = -k1x – k1d1(左侧)
y = -k2x + k2d2(右侧)
小车原点出发的两条线如下:
y = -k1x
y = -k2x
设如图菱形三个点坐标分别为(x1,y1),(x2,y2),(x3,y3)根据以上条件可以得到菱形这三个点坐标:
( k1d1/(k2-k1) , k1k2d1/(k1-k2) )
( k2d2/(k2-k1) , k1k2d2/(k1-k2) )
( (k1d1+k2d2)/(k2-k1) , k1k2*(d1+d2)/(k1-k2) )
则目标点(菱形中心)的坐标为( (x1+x2)/2 , y3/2 )
在gazebo中仿真turtlebot在跑道行驶
在gazebo中打开机器人模型并使用跑道地图,放置小车在如图位置:
此时通过小车上的相机获取当前图像如下:
图像处理函数得到以下线:
[array([ -1.30674284, 644.27895385]), array([ 0.44303425, 212.17684959]), array([ 2.1778871 , -215.65432416])]
在这个仿真环境中,d的值为0.5,经过计算,得到目标坐标为(0.031249525311438764 0.20417794911561207),接下来,我们用运动方程得到的计算值做为小车运动控制,最终小车运动的结果如下:
由结果可以看到,机器人回到了跑道的中间位置。
6.3.6 策略调整
实际仿真过程中,发现有几个地方需要特殊处理
1, 小车位置接近中间,方向接近正确方向时,如图所示:
此时判断出跑道和计算出目标坐标,以及需要转动角度如下:
跑道:[array([ -2.60406894, 967.10114694]), array([ -0.87817531, 541.50302416]), array([ 0.88219888, 105.75147489])]
目标:0.1647396886853823 0.0617375855904257
角度:20.5439092745
大致范围是|x|<0.2,|y|<0.2,角度Q<25度,此时如果按求出值做调整,偏离中心较大
因此,需要程序中对这种情况作判断,此时取x=0.5,y=0,四元数(0,0,0,1),这样保证小车继续沿正确方向行走
2, 小车距离某一线较近,角度与线小于45度时
以上研究还不能完全使小车跑完整个赛道,需要不断调整各种请款,留待后续有时间研究