python机器人编程——基于单目视觉、固定场景下的自动泊车（下）

一、前言

本篇来讨论一下在固定场景下，如何仅通过单目视觉，实现差速小车的自动停靠，这种方式实现成本比较低，可适应的范围比较广，比如可以应用于小车的自动充电桩寻找、工位的最准等应用场景。我们还是尝试应用有限、浅显的数学对固定场景下，通过坐标的变换、几何的分析、结合单目摄像头获取的图像，进行相机位置（或者小车的位置）的估计，并控制差速小车行驶至我们预设的位置停靠。本篇根据上篇已经识别的位置，进一步实现小车的控制部分。

二、主要思路

由于我们讨论的是简单场景，在知道了当前小车位置和方向角后，要行驶至固定点位，最直接粗暴的方法是走直线了，我们可以这样进行路线的控制：

step0 设定一个中间位置

由于视觉判断出来的位置毕竟准确的不高，我们有必要设立一个在目标停车区前面设置一个中间位置，用于最后停进去前进行缓冲，稍微精确的进行位置调整，类似两个空间站对接的场景。如上图绿色点，例如这个坐标如设为（0，-2500）单位mm，目标点坐标为（0，-1500）。

step1 掉转马头

这一步是直接原地转向，根据视觉计算的位置和中间点位置，将当前方向角原地旋转调整至朝向中间点的位置。如下图：

应该调整的角度dtheta=theta_t1-theta_t0,其中，theta_t1是根据向量Pt0---->Pt1计算获取，向量的旋转角度计算程序如下：

def single_vangle(v1):
    """
    计算单个向量夹角，返回弧度和角度
    """
    dx1 = v1[2] - v1[0]    
    dy1 = v1[3] - v1[1]    
    
    fangle1 = math.atan2(dy1, dx1)    
    angle1 = round(fangle1 * 180/math.pi,2)    
    return fangle1,angle1
1
2
3
4
5
6
7
8
9
10

用程序计算小车在Pt0点应该旋转的角度如下：

def move2ready(location,theta,clientID,TTHandle,speed=0.15,tspeed=0.05,main_local=(0,-2500),r=0.16):
    """
    r:小车中心到轮子的距离
    """
    p0x=location[0]
    p0y=location[1]
    p1x=main_local[0]
    p1y=main_local[1]
    orders=[]
    angle1=theta+90
    print("当前小车角度：",angle1)
    print("当前小车坐标：",p0x,p0y)
    f,angle0=single_vangle([p0x,p0y,p1x,p1y])
    print("当前中间点角度：",angle0)
    dangle=angle0-angle1
    print("转角角度：",dangle)
    if dangle>=0:#顺时针转rv>0>lv
        dangle=dangle/180*math.pi#转弧度
        vr=tspeed
        vl=-tspeed
        #旋转时间=角度/角速度，角速度=线速度/r
        tita=abs(dangle/(vr*10*0.164))
        orders.append([tita,(vl,vr),clientID,TTHandle])
    else:
        dangle=-dangle/180*math.pi#转弧度
        vr=-tspeed
        vl=tspeed
        #旋转时间=角度/角速度，角速度=线速度/r
        tita=abs(dangle/(vr*10*0.164))
        orders.append([tita,(vl,vr),clientID,TTHandle])
        ######part1------接下部分###############
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

PS：如何控制小车按照既定的指令前进，比如：先让小车左转30度，再匀速直行1米，再右转20度…
这里我们可以通过设置一个独立的线程，将需要的一些先后顺序的控制指令封装在一个list里面，命名为orders，其结构如下：
orders=[[执行时间t，(执行的左右轮速度),小车的ID号，小车的反馈状态句柄],…]
然后编制一个顺序指令函数，来解释这个orders,并按一定的先后顺序根据计时时间下发到小车，这样就形成了一个顺序控制（CCS）过程：

def tick_tack(orders,circle=True):
    """
    按时间执行控制，orders=[[tita,(leftspeed,rightpeed),clientID]]
    """
    x,y,xe,ye,ang0,ang1=0,0,0,0,0,0
    start_time=time.time()
    for i in range(len(orders)):          
        tita=orders[i][0]
        lv=orders[i][1][0]
        rv=orders[i][1][1]
        clientID=orders[i][2]
        TTHandle=orders[i][3]
        if circle:
            ret, arr = sim.simxGetObjectOrientation(clientID, TTHandle, -1, sim.simx_opmode_blocking)
            if ret==sim.simx_return_ok:
                x=round(arr[0]*1000,2)
                y=round(arr[1]*1000,2)  
                ang0=round(arr[2],2) 
                print("角度：",arr)
                if ang0<0:
                    ang0=math.pi*2+ang0
        else:
            ret, arr = sim.simxGetObjectPosition(clientID, TTHandle, -1, sim.simx_opmode_blocking)
            if ret==sim.simx_return_ok:
                x=round(arr[0]*1000,2)
                y=round(arr[1]*1000,2)  
                ang0=round(arr[2],2)  
        send_LR(clientID,lv,rv)            
        time.sleep(tita)
        if circle:
            ret, arr = sim.simxGetObjectOrientation(clientID, TTHandle, -1, sim.simx_opmode_blocking)
            if ret==sim.simx_return_ok:
                xe=round(arr[0]*1000,2)
                ye=round(arr[1]*1000,2)
                ang1=round(arr[2],2)  
                if ang1<0:
                    ang1=math.pi*2+ang1
                if ang1<ang0:
                    aspeed=(ang1+math.pi*2-ang0)/tita                    
                #speed=math.sqrt((xe-x)**2+(ye-y)**2)/tita
                #print("线速度(mm)：",speed)
                else:
                    aspeed=(ang1-ang0)/tita
                
                print("角速度(mm)：",aspeed)
                print("角度：",arr)
        else:
            ret, arr = sim.simxGetObjectPosition(clientID, TTHandle, -1, sim.simx_opmode_blocking)
        
            if ret==sim.simx_return_ok:
                xe=round(arr[0]*1000,2)
                ye=round(arr[1]*1000,2)                     
                speed=math.sqrt((xe-x)**2+(ye-y)**2)/tita
                print("线速度(mm)：",speed)
                
        send_LR(clientID,0,0)
        send_LR(clientID,0,0)
        time.sleep(0.01)
        
    end_time=time.time()   
    print("用时：",end_time-start_time)
    return True,end_time-start_time
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

step2 直线匀速前进

掉转朝向完成后，我们就控制小车左右轮速为相等，计算好直线移动至中间位置的时间，利用计算机监控时间，倒计时停止后，即为中间位置。
此处比较好理解，只要计算一下当前点Pt0到中间点Pt1的距离，并且设定好合适的速度，就可以计算小车应该行进的时间，于是生成指令加入orders序列：

######part2------接part1部分###############
    #计算前进时间
    distance=math.sqrt((p0x-p1x)**2+(p0y-p1y)**2)
    vl=speed
    vr=speed
    tita=distance/(vl*10*24)
    orders.append([tita,(vl,vr),clientID,TTHandle])
    ######part2------接下部分###############
1
2
3
4
5
6
7
8

step3 调整姿态

在中间位置附近了，就可以根据当前的朝向，原地旋转小车，将小车朝向调整至垂直面向目的区域，并且下车的控制由模型计算控制转为视觉导航为主的位置精调控制。
这一步跟第一步是一样的只要调整好小车方向至中间点至原点向量的角度就可以：

######part3------接part2部分###############
#计算中间点位置旋转
    dangle=90-angle0
    if dangle>=0:#顺时针转rv>0>lv
        
        dangle=dangle/180*math.pi#转弧度
        vr=tspeed
        vl=-tspeed
        #旋转时间=角度/角速度，角速度=线速度/r
        tita=abs(dangle/(vr*10*0.164))
        orders.append([tita,(vl,vr),clientID,TTHandle])
    else:
        dangle=-dangle/180*math.pi#转弧度
        vr=-tspeed
        vl=tspeed
        #旋转时间=角度/角速度，角速度=线速度/r
        tita=abs(dangle/(vr*10*0.164))
        orders.append([tita,(vl,vr),clientID,TTHandle])
    print("bigen move:",i,orders) 
    t=threading.Thread(target=tick_tack,args=(orders,))
    t.setDaemon(True)
    t.start()
    return t
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

最后创建一个新的线程，将生成的顺序控制指令orders用我们的顺序控制函数tick_tack执行。小车就会执行一些列的动作，移动到中间点附近了！

step4 视觉匹配

类似于导弹的地形匹配，我们利用单目视觉获得的图像，在小车前进中进行位置角度的调整，原则上是使得固定二维码中心始终在图像的中心位置，且其它参考点P1、P2相对中心点位置距离比例是相等的，垂直、横向二维码点不产生比例偏移。直至小车运动至目标位置停止。
这个问题其实就是一个阶跃的响应控制问题：

这里我们还是利用之前博文中所用到的预测控制思想去控制小车至目标设定值，上图红线所示。这里也可以通过PID等其它控制手段，实现小车位置的精调回正。

三、效果

移动到中间点：

最后精细控制至目标点：

四、全篇总结

至此，初步完成了在简单固定场景仿真环境下，基于单目视觉的自动停靠控制，通过测试，即使在理想的仿真环境下面，基于单目视觉简单的定位也存在着一定的误差，需要反复的进行调整参数，由此可见应用于实际场景，相对情况要复杂很多，单单利用单目视觉和简单的特征识别处理在实际应用场景还是不够的，要引入更多的信息源（如激光传感、深度传感）、使用优化算法来提高系统的可靠性和准确性。
后续还有很多改进的地方，让我们继续深入研究吧！