ROBOMASTER TT巡线.3

云深无际

发布于 2021-04-14 11:03:11

1.1K0

发布于 2021-04-14 11:03:11

文章被收录于专栏：云深之无迹

继续接着上篇文章。我们现在为止已经获得处理好的二值化图像了，接着就是提取线的特征了。

这里我们插一个小细节，就是图像的方向不对

怎么处理？

可能最牛逼的做法就是，飞机横着飞了，相对的图像就正了。

emmmm，那既然都有这个相对的想法了，我们为什么不调整图像。

我们现在在电脑上面看到的图像是这样的

上面这个算法就是，按照行来稀疏的提取一些像素行

我怕说不明白就画了一张图，emmm触控笔老是飘

首先我们认为飞机：

现在这个摆放的位置是正向

你会觉得飞机拍摄的图像是这样

但它是这样

在这里插一句关于图形的一个坐标，我们规定左上角为坐标的原点

img[x,y]，分别是像素点的行与列

那我们直接相对的把行列顺序也就是图像旋转90°来取样

由于运算量的关系，这里只取样5列。后期为了精度，可以取更多的列

这个是最简单的一个示意图

接着看这个，我们可以认为我们画的5个线里面有两条是落到轨迹里面的，那

我们这里用的算法是边缘检测算法

找到目标像素点的个数
记录对应目标像素点的索引（位置）
接着去把中心白线数值输出，接着与标准中心做差
得到的误差作为指导TT控制飞行的变量

def get_line_pos(img):
    img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, img_binary = cv2.threshold(img_gray, 200, 255, cv2.THRESH_BINARY)
    img_binary = cv2.erode(img_binary, None, iterations=1)

    color = []
    color.append(img_binary[:,60])
    color.append(img_binary[:,100])
    color.append(img_binary[:,160])
    color.append(img_binary[:,220])
    color.append(img_binary[:,260])

    result = []
    for i in range(0, 5):
        white_sum = np.sum(color[i] == 255)
        white_index = np.where(color[i] == 255)
        if white_sum > 6:
            white_center = (white_index[0][white_sum - 1] + white_index[0][0]) / 2
            result.append([1, white_center - 120])
        else:
            result.append([0, 0])
    return result, img_binary

我们现在来总结一下，目前为止的工作：

从下视摄像头获取到的视频流，作为单帧照片去处理
对每帧照片都进行灰度转换
进行二值化处理
对照片取5列像素
计算目前照片的轨迹与标准中线的误差值

我们现在到了控制算法的设计阶段：

我们要用到简单的PID控制：完成对TT的控制，但是我们还需要一些关于飞行器飞行时的姿态描述.

你可以按照这个图形来感觉一下这个相关的方位

Z轴正方向为前进方向

pitch()：俯仰，将物体绕X轴旋转（localRotationX）向下的话，会有一个前进的分力，然飞机前进。

yaw()：航向，将物体绕Y轴旋转（localRotationY）

就是机头的方向，我们可以认为指向哪里就往哪里飞。

roll()：横滚，将物体绕Z轴旋转（localRotationZ），这个是完成侧移动作，就是有点平移飞行的感觉。

也可以这样理解：

如果有一个人站在（0，0，0）点，面向X轴正向，头顶向上方向为Y轴正向，右手方向为Z轴正向，那么旋转角度和方向的计算方法如下： Yaw是围绕Y轴旋转，站在（0，0，0）点的人脚下是XOZ平面，以正角度为参数是向左转，以负角度为参数是向右转。 Pitch是围绕X轴旋转，站在（0，0，0）点的人脚下是XOY平面，以正角度为参数是向右倒，以负角度为参数是向左倒。 Roll是围绕Z轴旋转，站在（0，0，0）点的人脚下是YOZ平面，以正角度为参数是向后倒，以负角度为参数是向前倒。

首先是对机头方向的调整，就是飞行的方向

如果误差为0，就不要去发送让无人机运动的指令

误差不为零就提取里面的误差数×一个系数（负数），因为已经很偏了要抗拒这种改变，所以是负数

横滚也是一样的控制法，注意系数可以调节。

这种实时的控制方式是：前馈控制系统，其又为前馈控制的一种形式，是控制部分发出指令使受控部分进行某种活动，同时又通过另一快捷途径向受控部分发出前馈信号，受控部分在接受控制部分的指令进行活动时，又及时地受到前馈信号的调控，因此活动可以更加准确。

例如：要求将手伸至某一目标物，脑发出神经冲动指令一定的肌群收缩，同时又通过前馈机制，使这些肌肉的收缩活动能适时地受到一定的制约，因而手不会达不到目标物，也不致伸得过远，整个动作能完成的很准确。在这种调控过程中，前馈控制和反馈控制又是常常互相配合的。例如在脑指挥肌肉活动的过程中，肌肉和关节中的感受器将肌肉活动的信息反馈到脑，因此，脑可以对肌肉实际活动的情况与原先设计的动作要求之间的偏差进行分析，再对前馈信号进行调整，在以后再指令作同样的动作时，发出的前馈信号就更加准确，使完成动作能更接近设计的要求。

与前馈控制相比，反馈控制需要较长的时间，因为控制部分要在接到受控部分活动的反馈信号后才能发出纠正受控部分活动的指令，因此受控部分的活动可能发生较大波动。以神经系统对骨骼肌任意活动的控制为例，如果只有反馈控制而没有前馈控制，则肌肉活动时可出现震颤，动作不能快速、准确、协调地完成。

这个是判断飞机要不要降落，这个地方的逻辑写的不是很好

如果飞机速度快很容易在拐角出降落，后期代码优化

这里需要对控制的动作进行一些限制，防止过载现象

限制函数的写法

最后将实时的运动指令发给飞行器

第一个函数是从主机发送命令给TT

只是第二个函数的使用参数表

发送函数的使用就是这样，直接发送命令字符串

注意中间的延时，是用来让机器进入稳定状态的。因为机器会有一个初始化的过程。但是这个过程中，机器会有小距离漂移现象，目前还在优化。

在SDK的文档内也说明了延时的必要性

import robomaster
from robomaster import robot, flight
from numpy import *
ROLL_PARAM_P = -0.3
YAW_FEEDBACK = -0.7
FORWARD_SPEED = 15
THROTTLE = 0
robomaster.config.LOCAL_IP_STR = "192.168.10.2"
tl_drone = robot.Drone()
tl_drone.initialize()
tl_flight = tl_drone.flight
tl_camera = tl_drone.camera
def send_ctrl_cmd(cmd):
    tl_drone.action_dispatcher.send_action(flight.FlightAction(cmd))
def send_rc_cmd(a, b, c, d):
    tl_flight.rc(a=a, b=b, c=c, d=d)
def out_limit(val, min, max):
    if val > max:
        val = max
    elif val < min:
        val = min
    return val
def get_line_pos(img):
    img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, img_binary = cv2.threshold(img_gray, 200, 255, cv2.THRESH_BINARY)
    img_binary = cv2.erode(img_binary, None, iterations=1)
    color = []
    color.append(img_binary[:, 60])
    color.append(img_binary[:, 100])
    color.append(img_binary[:, 160])
    color.append(img_binary[:, 220])
    color.append(img_binary[:, 260])
    result = []
    for i in range(0, 5):
        white_sum = np.sum(color[i] == 255)
        white_index = np.where(color[i] == 255)
        if white_sum > 6:
            white_center = (
                white_index[0][white_sum - 1] + white_index[0][0]) / 2
            result.append([1, white_center - 120])
        else:
            result.append([0, 0])
    return result, img_binary
if __name__ == '__main__':
    send_ctrl_cmd('takeoff')
    time.sleep(5)
    tl_camera.start_video_stream(display=False)
    send_ctrl_cmd('downvision 1')
    time.sleep(3)
    while True:
        img = tl_camera.read_cv2_image()
        t = time.time()
        ret, imgbinary = get_line_pos(img)
        cv2.imshow("Drone", img)
        cv2.imshow("BIN", imgbinary)
        cv2.waitKey(1)
        if ret[0][0] == 0:
            yaw_out = 0
        else:
            yaw_out = YAW_FEEDBACK * ret[0][1]
            roll_out = ROLL_PARAM_P * ret[2][1]
        if ret[0][0] == 0 and ret[2][0] == 0:
            send_rc_cmd(0, 0, 0, 0)
            send_ctrl_cmd('land')
            break
        roll_out = out_limit(roll_out, -20, 20)
        yaw_out = out_limit(yaw_out, -40, 40)
        send_rc_cmd(int(roll_out), int(FORWARD_SPEED),
                    int(THROTTLE), int(yaw_out))
        print('%f, %d, %d' % ((time.time() - t)*1000, roll_out, yaw_out))
        time.sleep(0.01)
cv2.destroyAllWindows()
tl_camera.stop_video_stream()
tl_drone.close()

开发的注意事项有：