ROS+GAZEBO田径机器人的仿真研究

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度时

以上研究还不能完全使小车跑完整个赛道，需要不断调整各种请款，留待后续有时间研究