模块化LEGO船试验

设计目标

水上无人船平台，搭载不同功能模块实现多种功能，环境监测、水下摄像、水质采样、危机预警、心海湖喂鱼等。

第一步，遥控器操作
第二步，远程可视化遥控操作
第三步，自主航行及完成任务
第四步，水上群体智能协作

主控制器

实验阶段，使用LEGO，后续采用树莓派。

LEGO51515 MINDSTORMS 套装
乐高® MINDSTORMS® 头脑风暴机器人发明家 51515 | MINDSTORMS® | LEGO.com CN

LEGO Mindstorms Intelligent Large Hub (88016)

7.4V，2100mAh锂电池
100MHZ M4 320KB RAM 1M Flash Processor
32MB of memory for programs, sounds, and content.
6个端口
内置六轴陀螺仪传感器

4个M电机
1个超声波测距传感器
1个颜色传感器

任务清单

• [ ] 陀螺仪传感器测试
• [ ] PID程序测试

船体结构搭建

设计原则：结构简单，成本低廉，方便实现，模块化，可快速拆装、组合成不同形态。
分层，分块，骨架，快拆。
重要功能模块优先。
稳定性、可靠性。

零件：LEGO TECHNIC 常用零件，饮料瓶，橡皮筋。

船体功能布局

不同功能尽量隔离、分区，相互不影响，降低耦合。

浮力模块

质量132g，浮力瓶：农夫山泉茶π 900ml。

浮力模块组合成不同形态，通过增加模块数量，可从水平面积、垂直高度两个方向扩展。

动力模块

驱动模块可快速拆装重组。除提供电机连接口外，结构、尺寸与浮力模块相同。

任务清单

• [x] 船体结构，螺旋桨加支撑。
• [ ] 螺旋桨保护罩，防缠绕。
• [ ] 增加主控防水结构。

动力及控制

转向结构

几种常见的船体转向方案，最终选择方案1，双电机差速转向。
优点：结构简单，便于LEGO零件实现，容易建模计算，控制方向。

任务清单

• [x] 原地转向
• [ ] 差速运动建模

动力及电机

目前使用双电机，如果船体增大，考虑使用四电机驱动。

LEGO 88013电机

动力模块
电机：88013
输入：最大转速200转/分钟
齿轮加速比，(20:12)*(20:12) = 约 2.8:1

功能电机
Lego Spike M 电机

任务列表

• [ ] 测量最大航速
• [ ] 测量推力，确定最佳齿轮比

手机APP遥控操作

缺点：操作手感不及实体遥控器；需要手机平台安装APP。
优点：控制界面及功能可以修改、定制；显示状态信息丰富；蓝牙、wifi多种连接方式。

实体遥控器操作

lego 88010
7个按键，一个可编程控制指示灯。

遥控器按键功能分配

遥控控制分为两种模式：驱动模式/功能模式。
驱动模式控制船体运动，功能模式控制功其他功能模块。

驱动模式

按键	按下	双击	长按
左+	前进，持续加速（松开后保持当前速度）	全速前进
左-	后退，持续加速（送开会保持当前速度）	全速后退
左红	电机速度为0	全力停止（可能反转）
右+	持续右转，松开后保持转向角	全速右转
右-	持续左转，松开后保持转向角	全速左转
右红	方向回正（走直线）
中间绿	蓝牙连接（未连接时)/切换控制模式
左红/右红	控制参数恢复默认设定
左+/ 左-	测量电压（遥控器指示灯提示）

功能模式

按键	按下	双击	长按
右+	释放缆绳	释放缆绳到最长
右-	收起缆绳	收起缆绳到最短
右红	停止缆绳动作

任务清单

• [ ] 遥控器改造，增加操作性。
• [ ] 增加新手模式
• [ ] 通过遥控器调整控制参数功能

电池管理

515151套装电机*2，75mm外径螺旋桨，设置转速 1000（最大值)，水中连续运转，直至主机关机。

电压及转速衰减曲线

6.8v，10分钟
7v，30分钟
7.5v，130分钟
满电，190分钟

功能清单

• [x] 电池续航测试
• [ ] 电池管理，电压过低报警、电量提示等。
• [x] 电池扩容，连接充电宝。

控制程序代码

开发环境及语言：嵌入式Python
Pybricks: Python coding with LEGO

主机代码

"""  
作者：Gexin  
版本：1.1  
最后修改时间：2020/7  
Lego 88010遥控器控制双马达及其他马达  
"""  
from pybricks.pupdevices import Remote  
from pybricks.parameters import Button  
from pybricks.tools import wait  
from pybricks.pupdevices import Motor  
from pybricks.parameters import Port, Direction  
from pybricks.parameters import Color  
from pybricks.hubs import PrimeHub  
import math  

Debug = True  # 调试模式用于输出信息  

'''  
控制循环相关 常量  
'''  
# 一个控制循环内，只能执行一种控制模式  
CONTROL_MODEL_DRIVE = 'drive_mode'  # 控制模式 : 行驶模式，控制两个驱动马达  
CONTROL_MODEL_FUNCTION = 'function_mode'  # 控制非驱动马达  
CONTROL_CYCLE = 10  # 循环控制周期时间（毫秒）  
BUTTON_PRESS_WAIT = 300  # 按键按下后，需要等待一段时间，防止下一个控制循环连按。  

'''  
行驶相关 常量  
'''  
# 速度  
ACCELERATION_SPEED_STEP = 5  # 每个控制循环, 马达加速幅度，越大加速越快  
DRIVE_SPEED_MAX = 1000 # 度/秒  
DRIVE_SPEED_MIN = -1000  
# 转向  
TURN_ANGLE_STEP = 0.5  # 每个控制循环, 转向角的变化步长  
TURN_ANGLE_MAX = 90.0  
TURN_ANGLE_MIN = -90.0  

'''  
端口相关 常量  
'''  
# 马达连接端口配置  
DRIVE_LEFT_MOTOR = Port.E  
DRIVE_RIGHT_MOTOR = Port.F  
Function_A_MOTOR = Port.A  
FUN_B_MOTOR = Port.B # 收放缆绳马达  
FUN_C_MOTOR = Port.C  
FUN_D_MOTOR = Port.D  

'''  
 颜色相关  
'''  
COLOR_BT_CONNECTED = 'cbc'  
color_dict = {  
    CONTROL_MODEL_DRIVE: Color.BLUE,  
    CONTROL_MODEL_FUNCTION: Color.YELLOW,  
    COLOR_BT_CONNECTED: Color.GREEN  
}  

'''  
声音相关  
'''  

'''  
================================== Class =============================================  
'''  

class DifferentialDrive:  
    """  
    差速马达驱动，两个马达差速转向。  
    """    def __init__(self):  
        self.left_motor = Motor(DRIVE_LEFT_MOTOR, Direction.COUNTERCLOCKWISE)  
        self.right_motor = Motor(DRIVE_RIGHT_MOTOR, Direction.CLOCKWISE)  

    def drive(self, speed, turn_angle):  #  
        '''  
        speed: 直行 前进/后退 速度  
        turn_angle:转向角度  
        '''        left_motor_speed = speed  
        right_motor_speed = speed  

        print("trun angle: ", turn_angle)  

        if speed > DRIVE_SPEED_MAX:  # 不能超过最大值  
            speed = DRIVE_SPEED_MAX  

        if turn_angle > TURN_ANGLE_MAX:  #  
            turn_angle = TURN_ANGLE_MAX  
        if turn_angle < TURN_ANGLE_MIN:  
            turn_angle = TURN_ANGLE_MIN  

        # 转向需要的左右马达速度差 有正负之分 %        differential_percent = turn_angle / TURN_ANGLE_MAX  
        print("differential percent: ", differential_percent)  

        # 前进速度为0，原地转向; 或正在原地转向  
        if (right_motor_speed == left_motor_speed == 0 or right_motor_speed / left_motor_speed == -1):  
            print("原地转向")  
            right_motor_speed = DRIVE_SPEED_MAX * differential_percent  
            left_motor_speed = -right_motor_speed  

        else:  # 前进速度不为0，弧线转向  
            if turn_angle > 0:  # 右转  
                print("turn right ")  
                right_motor_speed = right_motor_speed * (1 - differential_percent)  
            if turn_angle < 0:  # 左转  
                print("turn left")  
                left_motor_speed = left_motor_speed * (1 + differential_percent)  

        if turn_angle == 0:  
            print("turn straight")  
            if right_motor_speed < left_motor_speed:  
                right_motor_speed = left_motor_speed  
            else:  
                left_motor_speed = right_motor_speed  

        # 控制马达  
        self.left_motor.run(left_motor_speed)  
        self.right_motor.run(right_motor_speed)  
        print("left - right motor speed: ", left_motor_speed, " - ", right_motor_speed)  

    def stop(self):  
        self.left_motor.stop()  
        self.right_motor.stop()  

# end of class Differential_driver  

class DriveController:  
    """  
    行驶控制器，将遥控器按键事件转换为控制命令。  
    用于 CONTROL_MODEL_DRIVE 模式。  
    """    def __init__(self, controlled):  
        self.controlled = controlled  
        self.drive_speed = 0  
        self.turn_angle = 0  
        print("Driver controller init ")  

    def para_forward(self):  # 修改参数，前进，加速  
        if self.drive_speed >= DRIVE_SPEED_MAX:  
            self.drive_speed = DRIVE_SPEED_MAX  
        else:  
            self.drive_speed += ACCELERATION_SPEED_STEP  
        print('left plus action')  

    def para_backward(self):  # 减速  
        if self.drive_speed <= -DRIVE_SPEED_MAX:  
            self.drive_speed = -DRIVE_SPEED_MAX  
        else:  
            self.drive_speed -= ACCELERATION_SPEED_STEP  
        print('left_minus_action')  

    def para_stop(self):  
        self.drive_speed = 0  
        self.controlled.stop()  
        print('left_action')  

    def para_turn_right(self):  # 右转  
        self.turn_angle += TURN_ANGLE_STEP  
        if self.turn_angle > TURN_ANGLE_MAX:  
            self.turn_angle = TURN_ANGLE_MAX  
        print('right_plus_action')  

    def para_turn_left(self):  # 左转  
        self.turn_angle -= TURN_ANGLE_STEP  
        if self.turn_angle < TURN_ANGLE_MIN:  
            self.turn_angle = TURN_ANGLE_MIN  
        print('right_minus_action')  

    def para_turn_straight(self):  # 方向回正  
        self.turn_angle = 0  
        print('right_action')  

    def center_action(self):  # 保留  
        pass  

    def handle_btn_pressed(self, btn):  # 执行遥控器的操作  
        action = self.button_action.get(btn)  
        action(self)  
        self.controlled.drive(self.drive_speed, self.turn_angle)  

    button_action = {  # 按钮 对应 功能  
        Button.LEFT_PLUS: para_forward,  
        Button.LEFT_MINUS: para_backward,  
        Button.LEFT: para_stop,  
        Button.RIGHT_PLUS: para_turn_right,  
        Button.RIGHT_MINUS: para_turn_left,  
        Button.RIGHT: para_turn_straight,  
        Button.CENTER: center_action  
    }  

# end of class Drive_controller  

# 功能控制器，用于 CONTROL_MODLE_FUNCTIONclass Function_controller:  
    def __init__(self):  
        self.crane_motor_speed = 300  

        self.crane_motor = Motor(FUN_B_MOTOR, Direction.COUNTERCLOCKWISE)  
        print("Function_controller Driver controller init ")  

    def left_plus_action(self):  
        pass  

    def left_minus_action(self):  
        pass  

    def left_action(self):  
        pass  

    def release_cable(self):  # 放缆绳  
        self.crane_motor.run(self.crane_motor_speed)  

    def retract_cable(self):  # 收缆绳  
        self.crane_motor.run(-self.crane_motor_speed)  

    def stop(self):  
        self.crane_motor.stop()  

    def center_action(self):  
        pass  

    def handle_btn_pressed(self, btn):  
        action = self.button_action.get(btn)  
        action(self)  

    button_action = {  
        Button.LEFT_PLUS: left_plus_action,  
        Button.LEFT_MINUS: left_minus_action,  
        Button.LEFT: left_action,  
        Button.RIGHT_PLUS: release_cable,  
        Button.RIGHT_MINUS: retract_cable,  
        Button.RIGHT: stop,  
        Button.CENTER: center_action  
    }  

# end of class Drrive_controller  

# 系统控制，派发遥控器的各种事件  
class Sys_controller:  
    boat = DifferentialDrive()  
    drive_controller = DriveController(boat)  
    function_controller = Function_controller()  

    def __init__(self, remote, sound_effect):  
        print('init Controller...')  
        self.sound_effect = sound_effect  
        self.remote = remote  
        self.set_control_mode(CONTROL_MODEL_DRIVE)  

    def set_control_mode(self, mode):  
        self.control_model = mode  
        self.remote.light.on(color_dict.get(mode))  

    def left_plus_action(self):  
        pass  

    def left_minus_action(self):  
        pass  

    def left_action(self):  
        pass  

    def right_plus_action(self):  
        pass  

    def right_minus_action(self):  
        pass  

    def right_action(self):  
        pass  

    def center_action(self):  
        self.switch_control_mode()  
        print('center_action')  

    def switch_control_mode(self):  # 两种控制模式切换  

        if self.control_model == CONTROL_MODEL_DRIVE:  
            self.control_model = CONTROL_MODEL_FUNCTION  
            self.remote.light.on(color_dict.get(self.control_model))  
            sound_effect.play_high_beep(2)  
            print("switch to function mode")  
            wait(BUTTON_PRESS_WAIT)  
            return  

        if self.control_model == CONTROL_MODEL_FUNCTION:  
            self.control_model = CONTROL_MODEL_DRIVE  
            self.remote.light.on(color_dict.get(self.control_model))  
            sound_effect.play_high_beep(1)  

            print("switch to drive mode")  
            wait(BUTTON_PRESS_WAIT)  
            return  

    button_action = {  # 按键事件，对应的方法  
        Button.LEFT_PLUS: left_plus_action,  
        Button.LEFT_MINUS: left_minus_action,  
        Button.LEFT: left_action,  
        Button.RIGHT_PLUS: right_plus_action,  
        Button.RIGHT_MINUS: right_minus_action,  
        Button.RIGHT: right_action,  
        Button.CENTER: switch_control_mode  
    }  

    # 处理按键事件  
    def handle_button_pressed(self):  
        # print("handle button pressed...")  
        # 按下的buttons  
        pressed_btn = my_remote.buttons.pressed()  
        for btn in pressed_btn:  
            action = self.button_action.get(btn)  
            print('action ', action)  
            action(self)  
            if self.control_model == CONTROL_MODEL_DRIVE:  
                self.drive_controller.handle_btn_pressed(btn)  

            if self.control_model == CONTROL_MODEL_FUNCTION:  
                self.function_controller.handle_btn_pressed(btn)  
                print()  

    def start(self):  # 进入控制循环  
        while True:  
            self.handle_button_pressed()  
            wait(CONTROL_CYCLE)  

# end of class Controller  

class SoundEffect:  # 音效类  

    def __init__(self, hub):  
        self.speaker = hub.speaker  

    def play_falling_notes(self):  
        for i in range(5, 10):  
            self.speaker.beep((15 - i) * 80, 100)  
            wait(10)  

    def play_rising_notes(self):  
        for i in range(5, 10):  
            self.speaker.beep((i) * 80, 80)  
            wait(10)  

    def play_low_beep(self, repeat=1, interval_times=100):  
        self.play_beep(100, repeat, interval_times)  

    def play_middle_beep(self, repeat=1, interval_times=100):  
        self.play_beep(500, repeat, interval_times)  

    def play_high_beep(self, repeat=1, interval_times=100):  
        self.play_beep(1000, repeat, interval_times)  

    def play_beep(self, frequency, repeat=1, interval_times=100):  
        for k in range(0, repeat):  
            self.speaker.beep(frequency, 100)  
            wait(interval_times)  

# end  

def initialize():  
    print('initialize begin...')  

'''  
程序运行入口  
'''  
# Initialize the hub.  
hub = PrimeHub()  

sound_effect = SoundEffect(hub)  
sound_effect.play_middle_beep(3)  

# 等待连接遥控器 直到成功，同步函数  
my_remote = Remote(None, 500000)  

sound_effect.play_rising_notes()  

my_remote.light.on(Color.GREEN)  
wait(1000)  

sys_controller = Sys_controller(my_remote, sound_effect)  

print("running....")  

sys_controller.start()

核心功能

任务清单

• [ ] 操作日志
  记录行驶参数、操作等信息。

• [ ] 轨迹记录

• [ ] 自主返回

• [ ] 自由巡航模式

• [ ] 遥控器信号丢失预警、报警

异常情况处理

遥控失灵
电池耗尽
船体被障碍物卡住
失去动力

应急措施：
安全绳
救援拖船
无人机牵引

模块化功能

机械臂

六自由度机械臂

图 机械臂结构设计

==注意：C轴转动会带动B轴转动，输入轴电机需要根据齿轮比反向抵消，才能保持另一个轴静止。==

图 机械臂结构LEGO实现，三个电机输入

重量（含3个Spike M电机）360克。

喷水

水下摄像

功能需求：

• 下潜一定深度
• 控制下潜、上浮速度
• 摄像头俯仰角度调整。
  

投喂模块

功能需求

• 可投放不同大小的固体颗粒食物，比如鱼食、玉米粒、馒头屑等。
• 每次投放数量可调节。
• 投放频率可调节。
• 投放距离可调节。
• 只需要单个电机即可完成投放功能。

不同投喂方案构想草图如上图。若想满足功能需求，有些方案已经可以排除掉，比如利用重力掉落、旋转容器、传送带等方案，无法控制投放距离（主要是无法向远处投放）。因此，应该采用基于抛洒的方案，要将颗粒抛洒出去，有多重方法，比如借助离心力、风力，但是不好控制方向和数量；还有利用运动惯性，比如抛石机，这种方法的缺点是结构占用空间比较大，颗粒装填复杂，距离不好调整；我们拟采用的方案是利用运动物体撞击颗粒，类似于枪机。
电机提供动力，让击锤拉动（皮筋变长），同时让颗粒进入击锤前方，击锤拉动到一定位置后，自由释放，依靠弹力复进，从而将颗粒物击出。电机连续运转，重复这个过程，完成自动投放过程。

![[模块化无人船实验平台-网站文章发布-.png]]

二自由度转盘

水平旋转，垂直俯仰

自动充电

四轴飞行器停泊

弹射器