CN111438691A - 仿生六足机器蟹控制系统 - Google Patents

Abstract

仿生六足机器蟹控制系统，属于多足机器蟹控制技术领域。本发明是为了解决机器蟹在海底复杂地形条件下自适应行走能力差的问题。包括：采用GPS定位模块采集六足机器蟹的定位信号；姿态传感器模块采集六足机器蟹的姿态角、角速度及加速度信号；视觉信息采集模块采集六足机器蟹行走过程中的视觉图像；机器蟹机体的六步行足和两尾翼足各关节通过舵机向执行机构传送控制指令并反馈相应的关节角度、关节速度及关节扭矩；控制单元对所有采集数据及预定行走路径进行处理，获得机器蟹每条步行足和尾翼足的期望运动轨迹，并计算获得步行足和尾翼足各关节的运动角度，再根据指令ID匹配传送给相应的舵机。本发明能够实现机器蟹在复杂地形条件下的自适应行走。

Description

仿生六足机器蟹控制系统

技术领域

本发明涉及仿生六足机器蟹控制系统，属于多足机器蟹控制技术领域。

背景技术

近年来，仿生六足机器蟹的相关研究快速发展。随着科学技术的提升，机器蟹控制系统的核心部分也得到了较快的发展，其控制系统也随之变的更加系统化、模块化，同时也对信息处理的实时性提出了较高的要求。

六足机器蟹基于海底复杂环境下的任务要求进行陆地模拟行走控制研究，为了完成复杂的任务，需要对崎岖不平的地面环境适应性行走，并实时对地面环境做出相应及时的反应动作，完成预定任务。

现有控制方法存在复杂地形条件下机器蟹自适应行走能力差，并且控制的实时性较差的问题。

发明内容

本发明目的是为了解决机器蟹在海底复杂地形条件下自适应行走能力差的问题，提供了一种仿生六足机器蟹控制系统。

本发明所述仿生六足机器蟹控制系统，包括控制单元1、GPS定位模块2、姿态传感器模块3以及视觉信息采集模块4，

所述GPS定位模块2用于采集六足机器蟹的定位信号；

姿态传感器模块3用于采集六足机器蟹的姿态角、角速度及加速度信号；

视觉信息采集模块4用于采集六足机器蟹行走过程中的视觉图像；

机器蟹机体的六步行足和两尾翼足各关节通过舵机向执行机构传送控制指令并反馈相应的关节角度、关节速度及关节扭矩；

控制单元1根据所述定位信号、姿态角、角速度及加速度信号、视觉图像及相应的关节角度、关节速度及关节扭矩对机器蟹的运动状态进行解算，同时根据机器蟹的预定行走路径，计算机器蟹每条步行足和尾翼足的期望运动轨迹，并计算获得步行足和尾翼足各关节的运动角度，根据指令ID匹配传送给相应的舵机。

根据本发明所述的仿生六足机器蟹控制系统，所述控制单元1通过485总线与机器蟹机体各关节设置的舵机进行通信。

根据本发明所述的仿生六足机器蟹控制系统，所述控制单元1通过串口无线通讯模块与上位机进行无线通信，上位机用于向控制单元1发送机器蟹的预定行走路径。

根据本发明所述的仿生六足机器蟹控制系统，电源管理模块5，包括蓄电池组和双尾翼太阳能板，用于为所述控制系统和机器蟹机体供电。

根据本发明所述的仿生六足机器蟹控制系统，姿态传感器模块3采集的姿态角包括航向角，

所述航向角提供机器蟹的实时行走方向，将实时行走方向与预定行走路径相应位置的方向进行计算，获得按照预定行走路径行走需要修正的偏差，根据所述需要修正的偏差确定修正后相应步行足的末端点位置；根据所述末端点位置，控制机器蟹机体的六步行足按照三角步态行走，行走步态顺序为：

左一步行足、右二步行足、左三步行足抬起，同时右一步行足、左二步行足、右三步行足支撑，机器蟹向前移动预设距离使相应步行足行至相应末端点位置；然后左一步行足、右二步行足、左三步行足落下变为支撑状态，同时右一步行足、左二步行足、右三步行足从支撑状态变为抬起状态，机器蟹再向前移动预设距离使相应步行足行至相邻下一次运动的相应末端点位置，完成一个步态周期；以所述步态周期循环运动，使机器蟹按照预定行走路径行走。

根据本发明所述的仿生六足机器蟹控制系统，所述步行足各关节的运动角度的计算方法包括：

根据GPS定位模块2提供的定位信号，确定机器蟹机体的位置信息；

建立D-H坐标系，根据机器蟹机体的位置信息与预定行走路径，计算每条步行足由当前位置移动到所述末端点位置，对应的三个关节需要转动的角度，实现机器蟹六步行足按所述步态周期的循环运动。

根据本发明所述的仿生六足机器蟹控制系统，使机器蟹按照预定行走路径行走还包括：

根据机器蟹机体的位置信息与预定行走路径进行计算，获得机器蟹当前位置与预定行走路径之间的位置偏差，对所述位置偏差根据正逆运动学计算方法进行修正计算，得到机器蟹由当前位置运动到所述末端点位置，对应的各步行足的关节舵机需要转动的角度。

根据本发明所述的仿生六足机器蟹控制系统，所述步行足末端点位置与相应关节需要转动的角度的计算方法包括：

P_x＝l₁c₁+l₃(c₁c₂c₃-c₁s₂s₃)+l₂c₁c₂

P_y＝l₁s₁+l₃(s₁c₂c₃-s₁s₂s₃)+l₂s₁c₂，

P_z＝l₂s₂+l₃(c₂s₃+c₃s₂)

式中，P_x,P_y,P_z为步行足末端点位置的三维坐标，l₁,l₂,l₃分别为每条步行足跟关节、膝关节和踝关节的关节长度，s_i表示sinθ_i，c_i表示cosθ_i，i＝1,2,3；θ₁,θ₂,θ₃分别为每条步行足跟关节、膝关节和踝关节需要转动的角度；

r为中间变量：

根据本发明所述的仿生六足机器蟹控制系统，当上位机向控制单元1发送机器蟹水面下潜和水下上浮指令时，通过485总线对机器蟹两尾翼足的四个关节舵机进行角度控制；通过舵机控制两尾翼足摆动向上时，机器蟹在水中进入下潜状态，同时在海底运动行走时控制两尾翼足保持向上摆动预定角度；通过舵机控制两尾翼足摆动向下时，机器蟹在水中进入上浮状态；由此实现机器蟹从海底运动到水面运动的转换。

根据本发明所述的仿生六足机器蟹控制系统，控制单元1包括STM32F407核心控制板。

本发明的优点：本发明用于机器蟹在海底崎岖不平的复杂环境下行走时的实时控制，能够高精度、快速灵活的完成预定任务。

本发明通过控制单元对机器蟹的运动状态进行解算以及对传感器数据进行实时信息处理，具有能耗低、速度快的优势；控制单元通过485总线与六足仿生机器蟹的每个关节舵机进行通信，通过总线控制驱动六条腿上的十八个关节舵机和两尾翼足的四个关节舵机，能够解决复杂地形机器蟹自适应行走问题。

本发明可制定机器蟹运动路线实现遥控或全自动运行，有效降低能耗，快速处理机器蟹位姿信息，并实时进行步态驱动；所述视觉信息采集模块实时反馈的视觉信息，保证了机器蟹具有良好的避障能力。

附图说明

图1是本发明所述仿生六足机器蟹控制系统的整体框图；

图2是采用本发明控制系统控制机器蟹海底行走的流程图；

图3是采用本发明控制系统对单步行足进行运动控制的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

具体实施方式一：下面结合图1说明本实施方式，本实施方式所述仿生六足机器蟹控制系统，包括控制单元1、GPS定位模块2、姿态传感器模块3以及视觉信息采集模块4，

所述GPS定位模块2用于采集六足机器蟹的定位信号；

姿态传感器模块3用于采集六足机器蟹的姿态角、角速度及加速度信号；

视觉信息采集模块4用于采集六足机器蟹行走过程中的视觉图像；

机器蟹机体的六步行足和两尾翼足各关节通过舵机向执行机构传送控制指令并反馈相应的关节角度、关节速度及关节扭矩；

控制单元1根据所述定位信号、姿态角、角速度及加速度信号、视觉图像及相应的关节角度、关节速度及关节扭矩对机器蟹的运动状态进行解算，同时根据机器蟹的预定行走路径，计算机器蟹每条步行足和尾翼足的期望运动轨迹，并计算获得步行足和尾翼足各关节的运动角度，根据指令ID匹配传送给相应的舵机。

本实施方式中控制单元1可以采用STM32控制处理器单元，对机器蟹的运动状态进行解算以及对传感器数据进行实时信息处理，具有能耗低、速度快的优势。

本实施方式中涉及机器蟹机体、传感器设备系统、控制系统、电源管理系统以及六步行足+两尾翼足；将机器蟹机体分上中下三层，从上至下依次为传感器系统平台、控制系统平台和电源管理系统平台，上层传感器平台包括视觉信息采集模块4、GPS定位模块2和姿态传感器模块3；下层包括电源管理系统中的蓄电池组；中间层为控制舱，通过线缆与所述上层设备进行连接。

所述视觉信息采集模块4与STM32控制处理器单元可通过网线进行连接，为机器蟹运动提供视觉信息；姿态传感器模块3可采用Mini AHRS微型姿态方位参考系统，可以在任意时刻、任意运动状态下精确输出载体的三个姿态角：航向角、俯仰角、横滚角，以及角速度、加速度等辅助传感信号，通过7针ODU接口可将姿态传感器模块3的输出信号以及电源引出，通过串口连接与控制单元进行数据处理，保证机器蟹位姿信息准确、实时的反馈给控制单元，使机器蟹能够灵活快速进行步态行走；所述GPS定位模块2，可采用基于GNSS的多系统高精度定位板卡GW-GNB110OEM，其尺寸小、功耗低、重量轻，具备8GB存储，能够保证对机器蟹实现高精度快速定位，保证机器蟹路径跟踪的精确性；所述视觉信息采集模块4可与控制单元1通过网线接口进行连接，对机器蟹运动过程中前方障碍信息进行反馈处理，保证机器蟹具有良好的避障能力。

六步行足和两尾翼足的关节舵机构成了系统中舵机驱动模块。

视觉信息采集模块4可采用宽动态1080P高清摄像头模组模块，与主控板控制单元通过网口连接，进行视觉检测识别、视觉避障任务。

所述GPS定位模块2位于机体重心垂向重合位置，保证定位信息的计算准确性；姿态传感器模块3位于机器蟹纵剖面的前半端上，保证航向计算的准确性；视觉信息采集模块4位于机器蟹前端，方便采集前方信息。

进一步，结合图1所示，所述控制单元1通过485总线与机器蟹机体各关节设置的舵机进行通信。所述控制单元1可以通过485总线控制驱动机器蟹六条腿上的十八个关节舵机和两尾翼足的四个关节舵机。

再进一步，结合图1所示，所述控制单元1通过串口无线通讯模块与上位机进行无线通信，上位机用于向控制单元1发送机器蟹的预定行走路径。所述串口无线通讯模块保证了PC端上位机与控制单元1进行无线通信。

串口无线通讯模块与STM32控制板可通过IO口进行连接，上位机将控制命令通过串口无线通讯模块发送给STM32控制板，实现机器蟹的无线操控。

再进一步，结合图1所示，电源管理模块5，包括蓄电池组和双尾翼太阳能板，用于为所述控制系统和机器蟹机体供电。

本实施方式中所述机器蟹采用电池和清洁能源混合供电。本发明采用嵌入式和低功耗设计，能够解决复杂地形下机器蟹自适应行走的问题，可制定机器蟹运动路线实现遥控或全自动运行，有效降低能耗，快速处理机器蟹位姿信息，并实时进行步态驱动；采用机器蟹双尾翼的太阳能板供电，可为机器蟹控制系统进行供电，使机器蟹在行走过程中充分利用太阳能提供动力，提高机器蟹续航能力。

所述双尾翼太阳能板包含在机体的上层传感器平台，与下层蓄电池组供电相结合构成清洁能源供电系统。

电源管理模块5可采用12V电源分别给控制系统、传感器模块和步行足动力驱动模块供电，其中还采用两块5V多晶太阳能电池板供电，单片电池板最大工作电流200mA，有效功率8W，可满足控制系统供电需求。传感器模块以及控制系统供电采用DC-DC电源隔离，以保证供电系统的稳定性，步行足舵机直接采用12V电源供电。

再进一步，结合图2所示，姿态传感器模块3采集的姿态角包括航向角，

所述航向角提供机器蟹的实时行走方向，将实时行走方向与预定行走路径相应位置的方向进行计算，获得按照预定行走路径行走需要修正的偏差，根据所述需要修正的偏差确定修正后相应步行足的末端点位置；根据所述末端点位置，控制机器蟹机体的六步行足按照三角步态行走，行走步态顺序为：

左一步行足、右二步行足、左三步行足抬起，同时右一步行足、左二步行足、右三步行足支撑，机器蟹向前移动预设距离使相应步行足行至相应末端点位置；然后左一步行足、右二步行足、左三步行足落下变为支撑状态，同时右一步行足、左二步行足、右三步行足从支撑状态变为抬起状态，机器蟹再向前移动预设距离使相应步行足行至相邻下一次运动的相应末端点位置，完成一个步态周期；以所述步态周期循环运动，使机器蟹按照预定行走路径行走。

姿态传感器模块3为机器蟹行走提供实时方位数据，准确预测机器蟹的行走姿态，对机器蟹的行走姿态计算提供参考依据。可根据D-H方法建立的坐标系进行步行足末端点位置的计算，确定修正后步行足末端点位置。

再进一步，结合图2所示，所述步行足各关节的运动角度的计算方法包括：

根据GPS定位模块2提供的定位信号，确定机器蟹机体的位置信息；

建立D-H坐标系，根据机器蟹机体的位置信息与预定行走路径，计算每条步行足由当前位置移动到所述末端点位置，对应的三个关节需要转动的角度，实现机器蟹六步行足按所述步态周期的循环运动。

GPS定位模块采用基于GNSS的多系统高精度定位板卡GW-GNB110OEM，对机器蟹机体位置进行实时定位，提供机器蟹的位置信息。根据D-H法建立的坐标系进行机器蟹每条步行足三个关节角度的计算，计算出每条步行足末端点落在行走路径上某一位置时三个关节角度需要转动的角度，结合所述三角步态行走实现机器蟹的行走。

图2中，机器蟹STM32F407核心控制板通过串口连接与上位机进行通信，接收上位机任务控制指令，同时处理机器蟹传感器数据，包括GPS定位机体位置信息，姿态传感器机器蟹航向信息，关节舵机反馈的关节角度、速度、扭矩等数据信息，视觉采集信息等，将采集到的机器蟹机体位置、行走航行、关节角度、速度等数据信息发送给控制板，控制板根据预定路径，计算机器蟹行走至预定路径时每条腿的运动轨迹，并计算采用三角步态或者其他步态行走时完成相应运动轨迹各关节需要运动的角度，使机器蟹按照任务指令实现步行足运动控制。将处理后的数据通过485总线命令发送给机器蟹步行足的各关节舵机，驱动步行足运动，执行行走指令，实现机器蟹的运动。

再进一步，结合图3所示，使机器蟹按照预定行走路径行走还包括：

根据机器蟹机体的位置信息与预定行走路径进行计算，获得机器蟹当前位置与预定行走路径之间的位置偏差，对所述位置偏差根据正逆运动学计算方法进行修正计算，得到机器蟹由当前位置运动到所述末端点位置，对应的各步行足的关节舵机需要转动的角度。

本实施方式有效提高了机器蟹运动行走相对预定行走路径的精确度。

图3为单步行足控制程序主要流程图。程序运行时，先对各传感器模块以及关节舵机初始化，如果接收到运动位置指令，通过运动控制算法计算单步行足三个关节舵机的转动角度，将数据指令通过485总线发送给关节舵机，每个舵机根据各自的ID接收相应的指令信息，完成各自的转动角度，如果接收到停止运动指令，则运动结束，程序停止执行，如果没有接收到结束指令，则返回继续执行运动指令，根据运动算法解算下一时刻步行足位置及关节舵机转动角度，循环往复，直至程序结束。

本发明中机器蟹核心控制板采用STM32F407开发板，实现机器蟹集中式控制，完成姿态传感器、GPS、视觉等信息处理以及关节舵机控制等任务，完成机器蟹行走控制和水面运动以及下潜时尾翼足角度控制。

在接收到设定机器蟹运动路径指令进行水下行走任务时，核心控制板根据机体位置和机器蟹航向信息计算出下一时刻的运动位置，并根据D-H法建立的坐标系进行正逆运动学计算，得出机体运动到下一位置时各步行足末端点位置和各步行足关节转动角度。

再进一步，所述步行足末端点位置与相应关节需要转动的角度的计算方法包括：

P_x＝l₁c₁+l₃(c₁c₂c₃-c₁s₂s₃)+l₂c₁c₂

P_y＝l₁s₁+l₃(s₁c₂c₃-s₁s₂s₃)+l₂s₁c₂，

P_z＝l₂s₂+l₃(c₂s₃+c₃s₂)

式中，P_x,P_y,P_z为步行足末端点位置的三维坐标，l₁,l₂,l₃分别为每条步行足跟关节、膝关节和踝关节的关节长度，s_i表示sinθ_i，c_i表示cosθ_i，i＝1,2,3；θ₁,θ₂,θ₃分别为每条步行足跟关节、膝关节和踝关节需要转动的角度；

r为中间变量：

再进一步，当上位机向控制单元1发送机器蟹水面下潜和水下上浮指令时，通过485总线对机器蟹两尾翼足的四个关节舵机进行角度控制；尾翼足呈扁平状，通过舵机控制两尾翼足摆动向上时，机器蟹在水中进入下潜状态，同时在海底运动行走时控制两尾翼足保持向上摆动预定角度，以克服机器蟹在海底运动时浮力产生的影响；通过舵机控制两尾翼足摆动向下时，机器蟹在水中进入上浮状态；由此实现机器蟹从海底运动到水面运动的转换。

综上，机器蟹关节舵机驱动系统采用485总线通讯舵机，可实现0°-360°及多圈任意角度转动，舵机采用全双工异步串行总线通讯方式，通过UART接口统一控制，最多可通过总线连接254个舵机，每个舵机设定不同的ID，机器蟹关节舵机ID为1-22，二十二个舵机可以统一运动也可以单个独立运动，机器蟹STM32控制板将处理好的指令信息通过总线发送给二十二个关节舵机，各个舵机根据各自的ID进行指令接收，完成相应的关节动作，六条步行足十八个关节舵机统一协调运动，并且舵机可提供速度、扭矩、角度等反馈信息，将这些信息反馈给控制单元，结合控制算法计算机器蟹实时行走位姿，实现机器蟹的行走控制。两条尾翼足四个关节舵机根据任务需求进行角度控制，实现机器蟹的上浮和下潜。

作为示例，控制单元1包括STM32F407核心控制板。

所述STM32F407核心控制板包括STM32F407ZGT6控制处理器，带有FPU的

32位

可在Flash存储器中实现零等待状态运行性能的自适应实时加速器ART加速器、主频高达168MHz，功耗低，具有8-14位并行照相机接口，速度高达54MB/s，运算速度快，可保证机器蟹多信息融合技术下的数据处理能力，实时进行运动控制。串口无线通讯模块的通讯天线可安装于机器蟹上层传感器平台，可采用E62-433T30D全双工无线数传模块，能够同时接收发送数据，同时具有低延迟、高响应的优点，能够确保快速传输数据，从而保证机器蟹遥控操作的实时性，并可通过无线数传模块对机器蟹设定指令进行全自动控制。

所述控制单元1发送的机器蟹指令可通过485总线通讯，传输到机器蟹步行足和尾翼足每一个关节舵机，根据指令ID匹配相应关节运动角度，从而实现对二十二个关节舵机的总线控制，并能保证舵机反馈信息实时传输给控制单元，从而保证机器蟹的多足协调运动。

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (10)

1.一种仿生六足机器蟹控制系统，其特征在于，包括控制单元(1)、GPS定位模块(2)、姿态传感器模块(3)以及视觉信息采集模块(4)，

所述GPS定位模块(2)用于采集六足机器蟹的定位信号；

姿态传感器模块(3)用于采集六足机器蟹的姿态角、角速度及加速度信号；

视觉信息采集模块(4)用于采集六足机器蟹行走过程中的视觉图像；

机器蟹机体的六步行足和两尾翼足各关节通过舵机向执行机构传送控制指令并反馈相应的关节角度、关节速度及关节扭矩；

控制单元(1)根据所述定位信号、姿态角、角速度及加速度信号、视觉图像及相应的关节角度、关节速度及关节扭矩对机器蟹的运动状态进行解算，同时根据机器蟹的预定行走路径，计算机器蟹每条步行足和尾翼足的期望运动轨迹，并计算获得步行足和尾翼足各关节的运动角度，根据指令ID匹配传送给相应的舵机。

2.根据权利要求1所述的仿生六足机器蟹控制系统，其特征在于，

所述控制单元(1)通过485总线与机器蟹机体各关节设置的舵机进行通信。

3.根据权利要求2所述的仿生六足机器蟹控制系统，其特征在于，

所述控制单元(1)通过串口无线通讯模块与上位机进行无线通信，上位机用于向控制单元(1)发送机器蟹的预定行走路径。

4.根据权利要求3所述的仿生六足机器蟹控制系统，其特征在于还包括：

电源管理模块(5)，包括蓄电池组和双尾翼太阳能板，用于为所述控制系统和机器蟹机体供电。

5.根据权利要求4所述的仿生六足机器蟹控制系统，其特征在于：

姿态传感器模块(3)采集的姿态角包括航向角，

所述航向角提供机器蟹的实时行走方向，将实时行走方向与预定行走路径相应位置的方向进行计算，获得按照预定行走路径行走需要修正的偏差，根据所述需要修正的偏差确定修正后相应步行足的末端点位置；根据所述末端点位置，控制机器蟹机体的六步行足按照三角步态行走，行走步态顺序为：

左一步行足、右二步行足、左三步行足抬起，同时右一步行足、左二步行足、右三步行足支撑，机器蟹向前移动预设距离使相应步行足行至相应末端点位置；然后左一步行足、右二步行足、左三步行足落下变为支撑状态，同时右一步行足、左二步行足、右三步行足从支撑状态变为抬起状态，机器蟹再向前移动预设距离使相应步行足行至相邻下一次运动的相应末端点位置，完成一个步态周期；以所述步态周期循环运动，使机器蟹按照预定行走路径行走。

6.根据权利要求5所述的仿生六足机器蟹控制系统，其特征在于：

所述步行足各关节的运动角度的计算方法包括：

根据GPS定位模块(2)提供的定位信号，确定机器蟹机体的位置信息；

建立D-H坐标系，根据机器蟹机体的位置信息与预定行走路径，计算每条步行足由当前位置移动到所述末端点位置，对应的三个关节需要转动的角度，实现机器蟹六步行足按所述步态周期的循环运动。

7.根据权利要求6所述的仿生六足机器蟹控制系统，其特征在于：

使机器蟹按照预定行走路径行走还包括：

根据机器蟹机体的位置信息与预定行走路径进行计算，获得机器蟹当前位置与预定行走路径之间的位置偏差，对所述位置偏差根据正逆运动学计算方法进行修正计算，得到机器蟹由当前位置运动到所述末端点位置，对应的各步行足的关节舵机需要转动的角度。

8.根据权利要求7所述的仿生六足机器蟹控制系统，其特征在于：

所述步行足末端点位置与相应关节需要转动的角度的计算方法包括：

式中，P_x,P_y,P_z为步行足末端点位置的三维坐标，l₁,l₂,l₃分别为每条步行足跟关节、膝关节和踝关节的关节长度，s_i表示sinθ_i，c_i表示cosθ_i，i＝1,2,3；θ₁,θ₂,θ₃分别为每条步行足跟关节、膝关节和踝关节需要转动的角度；

r为中间变量：

9.根据权利要求8所述的仿生六足机器蟹控制系统，其特征在于：

当上位机向控制单元(1)发送机器蟹水面下潜和水下上浮指令时，通过485总线对机器蟹两尾翼足的四个关节舵机进行角度控制；通过舵机控制两尾翼足摆动向上时，机器蟹在水中进入下潜状态，同时在海底运动行走时控制两尾翼足保持向上摆动预定角度；通过舵机控制两尾翼足摆动向下时，机器蟹在水中进入上浮状态；由此实现机器蟹从海底运动到水面运动的转换。

10.根据权利要求1至9中任意一项所述的仿生六足机器蟹控制系统，其特征在于：

控制单元(1)包括STM32F407核心控制板。

Images