一种基于仿生海龟两栖航行器的控制系统及控制方法 |
|||||||
| 申请号 | CN202310656847.4 | 申请日 | 2023-06-05 | 公开(公告)号 | CN117908420A | 公开(公告)日 | 2024-04-19 |
| 申请人 | 大连理工大学; | 发明人 | 杨云聪; 周波; 杨逸韬; 李禹泽; 姚子鸣; 徐明林; | ||||
| 摘要 | 一种基于仿生海龟两栖航行器的控制系统及控制方法，属于 电子 信息技术领域。该系统由一个主控芯片、三个从控芯片、若干 传感器 、 电机 及其 编码器 组成。主控芯片进行命令管理、 人机交互 、遥控功能，主要任务包括接收数据；根据具体航行目标和航行 姿态 ，采用基于嵌入物理量信息的神经网络的方法，实现路径规划并给出实时姿态调整方案；将方案通过串口通讯发给从控芯片。从控芯片A根据主控芯片的指令，用PWM 波形 和TTL电平具体控制四个步进电机和两个 舵 机，并接收步进电机实时姿态信息以实现闭环控制。从控芯片B负责传感器信息的采集与数据打包，并在主控芯片需要时进行数据传递，执行主控芯片的传感器开闭命令。从控芯片C作为 芯片组 的FAT文件系统存在，存储必要数据。 | ||||||
| 权利要求 | 1.一种基于仿生海龟两栖航行器的控制系统，其特征在于，所述控制系统包括主控芯片（1）、从控芯片A（2）、从控芯片B（3）、动力装置、传感器装置和从控芯片C（4）； |
||||||
| 说明书全文 | 一种基于仿生海龟两栖航行器的控制系统及控制方法技术领域背景技术[0002] 随着陆地资源的进一步开发，人类对于向海洋特别是深海发展的热情正在逐渐提高。而深海资源探测往往会需要无人航行器（UAV）来代替人类进行作业。目前的海洋航行器普遍采用常规潜艇式布局，操作较为简洁。但对于有更高的航行效率和自持能力的海龟仿生机器人而言，其控制复杂度有所提高，为了能够胜任更加复杂的姿态控制，更多的传感器，更多的电机对控制系统的设计和逻辑提出了更高的要求。 [0003] 仿生海龟类航行器的水翼控制需要持续不断的进行和持续不断的调整，也需要传感器数据持续不断的接收，还需要对需要采集的数据持续不断的存储。若让单个芯片实现和完成所有的任务，容易发生冲突，若直接采用操作系统分配任务，也容易因为任务的耦合问题产生矛盾。再者，若需要执行算力较大的任务，例如图像实时传输和识别时，单片机的芯片的工作量将会急剧增大，会导致机器人反应迟缓。难以应对复杂多变的海况。 发明内容[0004] 本发明的目的：设计一套针对前龟鳍二自由度 ，后龟鳍单自由度的仿生海龟机器人的控制系统，并使该控制系统具有一定的可扩展性和可靠性。采用分布式控制系统，以主控制芯片调度分控制芯片，分控制芯片分别负责传感器处理，电机控制和数据储存，提高系统冗余度，并能够对指令和外界环境做出有效和快速的响应。本设计的核心思路是将主控制芯片的任务尽量外包给从控制芯片，只负责基本的指令传输和任务调度。 [0005] 其针对航行器的下潜和水面巡航状态设置了相应的传感器和动力设备，且采用一主机三从机的分布式硬件连接；控制逻辑在于：动力控制，传感器管理，文件管理由三块芯片分别负责，并由主控芯片基于操作系统调度，而操作系统的时钟通过从控芯片产生，设备间通讯根据水下和水面设计了两套命令语法。另外，首创的基于PINN神经网络的GPS陀螺仪定位耦合算法也为专利的重要组成。 [0006] 控制系统逻辑部分如下：航行器有两种模式：水面模式，水下模式。 [0007] 有两种指令：直接遥控指令和预设目标指令。 [0008] 水面和水下模式的指令不同，若对水面模式的控制系统发送水下模式的命令，控制系统会拒绝执行。例如水面航行至预设坐标点的指令为ST+set(x,y)，而水下航行至预设坐标点的指令则为ST+set(x,y,z)他们分别只在各自的模式下运行。 [0009] 本发明采用的技术方案：一种基于仿生海龟两栖航行器的控制系统，所述控制系统包括主控芯片、从控芯片A、从控芯片B、动力装置、传感器装置和从控芯片C。 [0010] 所述动力装置包含四个步机电机、第一舵机、第二舵机和压载水舱，步机电机通过编码器连接从控芯片B，两个舵机通过舵机控制板连接从控芯片B，压载水舱通过驱动器连接从控芯片B，仿生海龟两栖航行器的左前翼、右前翼上各设置两个步机电机，第一舵机、第二舵机分别设置在左尾翼、右尾翼上，压载水舱设置在航行器的背部。 [0011] 传感器装置包含陀螺仪、声纳模块、水压水深传感器模块、红外传感器模块、GPS模块，从控芯片A通过IIC连接陀螺仪和水压水深传感器模块，通过ADC连接红外传感器模块，通过串口连接GPS模块和声纳模块。 [0012] 从控芯片C连接时钟模块，主控芯片连接从控芯片A、从控芯片B和从控芯片C；从控芯片A数据连接上位机。 [0013] 一种基于仿生海龟两栖航行器的控制系统的控制方法，主控芯片统一调度从控芯片A、从控芯片B和从控芯片C，同时基于GPS数据、地磁数据、加速度数据、角加速度数据和水深数据，使用PINN神经网络进行加权修正得到航行器的航行坐标、位置坐标。 [0014] 从控芯片B控制动力装置、从控芯片A控制传感器装置。 [0015] 从控芯片C采集和存储数据，同时生成航行器的各项参数的曲线，用于规划航行任务和分析航行稳定性分析。 [0016] 所述的控制方法，从控芯片A基于PID对主控芯片给出的调整命令进行平滑的过度。 [0018] 在硬件连接设计上，主控制芯片和三块从控制芯片分别连接，三块分控制芯片之间相互独立，并分别与动力系统、传感器、文件存储区相连。这种分工有效的平衡了机器人的多种任务之间的压力。对于水翼海龟机器人这种水下平台，其任务载荷多种多样，因此该设计能够提高机器人的可移植性和升级空间——例如，若需要更换传感器型号或类别，只需要修改传感器芯片的代码即可。 [0019] 在软件方面，主控制板采用简易操作系统实现多任务调度，同时OS时钟较为特殊的由从控制芯片来负责，这种独立软件时钟源的设计也是为了提高系统的模块化。动力系统控制最核心的部分是基于PID对主控芯片给出的不断调整的命令进行平滑的过度，使海龟在微调航行路径的过程中不会出现明显的振荡。传感器数据采集则采用时钟中断定时采集，这样可以稳定的保证数据达到最大精度的同时又等间隔，利于后续的计算。将所有传感器数据处理打包后以数据帧的方式发送给主控板，确保主控板随时都能够访问当下的具体航行数据。再者，创新性的使用一块处理器专门处理采集数据，该文件存储芯片的主要作用是生成航行器的各项参数的曲线，以用于航行规划的任务实现，也便于观察曲线的状态来对其航行稳定性做分析，还可以对具体任务所需采样的数据进行存储，以用于科学研究。 [0020] 本系统为航行器在水下和水面的控制设计了二维和三维两种分别适用于水面和水下的坐标系。两种坐标系都以海龟头部为x轴正方向，所有的路径指令都被简化和离散为向坐标点航行的指令，大大简化的巡航控制的难度。通过上位机的指令，主控芯片能够通过对从控芯片的管理实现模式切换。启动对应传感器和对应的航行姿态控制。坐标系的建立又基于GPS和陀螺仪数据的算法实现，在陀螺仪持续跟踪航行器位置的情况下，利用GPS的粗定位能力进行实时轨迹修正。为了耦合GPS和陀螺仪由于累计误差产生的不一致，本发明首次尝试使用PINN神经网络计算航行坐标加权值，消除部分原本的系统误差，提高航行器位置感知的精度和稳定性、连续性。 [0021] 本发明是同类仿生海龟航行器控制系统中首个采用多CPU分布式控制方法的控制系统，将通讯、控制、文件系统交给从CPU负责，解决了同类航行器单CPU任务繁重的问题，提高了效率。同时，首次在仿生海龟航行器中使用基于GPS和陀螺仪的虚拟空间坐标系的运动控制逻辑，并首次基于PINN神经网络计算加权修正值，使得该型机器人能够实现精确的自主位置感知能力。 [0022] 本设计的有益效果在于：首次在仿生海龟水翼航行器中应用分布式控制系统设计，对于运动自由度较高，控制任务复杂繁重的同类机器人，能够更简单的实现设备管理、提高航行器的改装空间和简易度，提高航行器的硬件可扩展性和软件升级便利性。创新性的采用一个cpu专门负责将数据进行文件管理的方式，使得文件系统相对独立于控制系统，减少主cpu的占用，有效提高了效率。另外，该控制系统的控制逻辑利用基于独创的PINN神经网络的陀螺仪GPS耦合算法实现航行器航迹基于坐标系的跟踪，消除部分系统误差，通过嵌入物理信息对神经网络进行惩罚，提高了拟合回归的速度，并提高了航行器对自身航迹掌握的精确性、稳定性、连续性。附图说明 [0023] 图1是一种基于仿生海龟两栖航行器的控制系统的结构示意图。 [0024] 图2是一种基于仿生海龟两栖航行器的控制系统的逻辑设计图；（a）为系统初始化方式的示意图；（b）为系统运行方式的示意图；（c）为GPS陀螺仪耦合定位方法的示意图。 [0025] 其中，1、主控芯片，2、从控芯片A，3、从控芯片B，4、从控芯片C，5、步进电机，6、编码器，7、第一舵机，8、第二舵机，9、舵机控制板，10、压载水舱，11、上位机，12、陀螺仪，13、声纳模块，14、水压水深传感器模块，15、红外传感器模块，16、GPS模块，17、时钟模块。 具体实施方式[0026] 下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。 [0027] （1） 硬件连接方式如下（如图1所示）：A. 主控芯片：STM32F407ZGT6芯片及其电路板，从控芯片A：STM32F103C8T6及其开发板，从控芯片B：STM32F103C8T6及其开发板，从控芯片C：STM32F103C8T6及其开发板。主控芯片通过UART总线与从控芯片连接。其中，通过UART1的RX、TX与USART1的RX、TX连接，通过UART1的RX、TX与USART2的RX、TX连接，通过UART1的RX、TX与USART3的RX与TX连接。 [0028] B. 动力控制部分，有四个MG42S1步机电机及其编码器EMM42，其编码器的串口分别与从控芯片B连接。两个DCS3225舵机与舵机控制板连接，舵机控制板串口亦与从控芯片A连接。压载水舱的驱动器与从控芯片B连接。四个步进电机两两一组分别设在海龟航行器的左前翼、右前翼，分别负责两个前翼的拍旋和位旋；两个舵机则设在海龟航行器的两尾翼，分别负责海龟的双足拍打。 [0029] C. 传感器有mpu6050六轴姿态陀螺仪三轴加速度传感器（以下简称陀螺仪）、KS114声呐模块、M10水压水深传感器、M18漫反射红外传感器模块、GPS模块。陀螺仪和水压水深模块通过IIC与从控芯片A连接，红外传感器模块通过ADC与从控芯片A连接，声纳模块和GPS模块通过串口与从控芯片A相连。 [0030] D. DS3231时钟模块与从控芯片C连接。 [0031] （2）控制系统逻辑如下：S101初始化：初始化操作系统、任务控制模块，并向从控芯片A、从控芯片B、从控芯片C发送初始化指令； S102待命：基于从控芯片A的GPS数据的位置判断，微调自身位置保持不变。 [0032] S103规避：基于从控芯片A的红外传感器数据和航速数据进行紧急规避任务；S104数据请求：向从控芯片A、从控芯片B发送数据请求，获得从控芯片A的传感器数据和从控芯片B的电机模式数据； S105存储使能：向从控芯片C发送数据通知，开启对应DMA口，持续向从控芯片C通过串口传输指令； S106存储关闭：向从控芯片C发送数据终止通知，关闭对应DMA口，停止向从控芯片C通过串口传输指令； S107航向航速指令：向从控芯片B发送航向航速指令，使从控芯片B驱动步进电机 进行水翼划行，指令为具体的方向矢量，即使航行器下一秒具体需要的加速度大小和方向矢量； S108遥控航行指令处理：上位机发送航行指令给从控芯片A后，通过从控芯片A的 遥控指令触发，并执行航行指令，指令有航向目标位置和直接控制两种——前者给定具体GPS坐标，后者给定具体前后、左右、上下命令。主控芯片在收到指令后会基于GPS数据、地磁数据、加速度数据、角加速度数据和水深数据，使用PINN神经网络进行加权修正得到航行器的航行和航向坐标，建立坐标系、规划航迹，并持续向从控芯片A发送S104指令以获得实时位置与速度情况，然后向从控芯片B发送S107指令修正航行。该指令的具体执行受到S109的模式切换影响，根据水面和水下具体调整； S109遥控模式切换指令处理：除了航行指令，还有模式切换指令，即水面和水下模式。接收到该指令后，主控芯片将持续通过S107和S104指令进行跟踪，控制压载水舱和电机实现切换目标； S110航行指令满指令：即暂缓执行下一个发来的航行指令并通过从控芯片A将该 信号传输至上位机； S111规避2：基于声呐和陀螺仪数据进行障碍物规避； 从控芯片A执行以下任务： S201初始化：收到主控芯片指令后，初始化各传感器，和上位机建立通讯； S202数据采集：循环将数据按照时间为单位存入特定存储区，并删除溢出数据； S203数据传输：将数据循环向主控芯片打包发送，或接收到主控芯片的请求后发 送； S204传感器开闭：水面模式时，关闭深度传感器和声呐模块，水下模式时，关闭红外传感器模块； 从控芯片B执行以下任务： S301初始化：收到主控芯片指令后，配置电机参数和任务控制模块； S302矢量驱动：根据主控芯片的具体力矢量命令（加速度和转矩）、解算具体每个动力机构（步进电机、舵机）所需的力和推动方案，并执行；航行器的两个前翼上分别设置两个电机，两个电机基于传动机构分别负责前翼的拍旋和位旋运动，两个舵机则分别设置在两个后翼上，用于控制两个后水翼的拍打。航行器加速度大小的改变由步进电机周期性配合运动的速度决定，方向的改变则由负责位旋的步进电机的往复运动的旋转角度决定。 [0033] S303标量驱动：根据主控芯片的直接控制命令，执行前后、左右、上下航行，并等待下一次命令；S304模式切换：调整压载水舱，切换步进电机操控算法； S305规避，根据主控芯片的预估障碍物位置实施开环控制紧急规避，必要时会进 行后撤； 从控芯片C执行以下任务： S401初始化：收到主控芯片指令后，初始化存储区和FAT文件索引模块； S402开始存储：使用FAT文件系统将DMA数据进行存储； S403停止存储：停止存储。 [0034] 以下将结合附图2详细描述这些芯片如何使用命令完成初始化、运行、定位任务的。为便于查阅，以下用前述命令的编号作为任务名。例如S201表示从控芯片A的初始化任务。 [0035] 结合图2（a），主控芯片执行S101初始化任务，完成自身初始化后，向从控芯片A、从控芯片B、从控芯片C分别发送初始化命令，三块从控芯片开始各自初始化结合图2（b），从控芯片A采集姿态传感器、声纳模块、水压水深传感器模块、红外传感器模块、GPS模块的数据，接收上位机的指令数据，也就是执行S202数据采集任务，然后执行S203数据传输任务，或在主控芯片执行S104时，给予特定的数据。主控芯片此时会基于传感器装置的数据和上位机任务执行S102、S103、S105、S106、S107、S108、S109、S111，若主控芯片的任务满了，那么就会向从控芯片A发送S110的反馈信号，使从控芯片A延时一段时间后再发送上位机命令。当执行存储使能S105后，主控芯片会向从控芯片C发送通知，使之执行S402，还会通过DMA方法使得从控芯片C持续收到从控芯片A的传感器装置的数据。当执行存储关闭S106后，则关闭DMA，并让从控芯片C执行S403。而当主控芯片执行S102、S103、S107、S108、S109、S111后，则会与从控芯片B通讯，使之执行S202、S203、S204、S205任务，操控步进电机、舵机和压载水舱，从而实现航行器的行为控制。而主控芯片向从控芯片下达任务的安排和规划，则由主控芯片的操作系统和航行逻辑进行规划调度。 [0036] 结合图2（c），具体描述主控芯片航行逻辑的架构。分别基于GPS和传感器的数据创新性的设计了一章加权修正算法。该加权修正方法考虑到GPS和陀螺仪误差的产生原因，即：GPS的时间信号误差会导致GPS的定位出现波动，若动态采样并使用卡尔曼滤波处理，可以得到厘米级稳定的相对精度值，但考虑到航行器的运动会为滤波带来不确定性，从而影响GPS信号处理的精度，又考虑到水下航行器深度越大，对于GPS信号的连续性破坏也越大，GPS坐标可参考性会降低；反过来，地磁会影响陀螺仪的零位移误差，而水深亦会对地磁的测量值产生影响。综上，GPS和陀螺仪的结果存在复杂的耦合误差，简单的加权值修正并不能真正实现高精度融合定位。故采取将陀螺仪获得的地磁数据，加速度和角加速度数据，以及水深数据和GPS两轴坐标及其标准差数据以六个采样单位时间为一组，组合为一个14*6的矩阵，作为PINN神经网络的输入层，通过隐藏层输出到维度为3的列向量中，输出结果即融合计算后的三坐标位置加权修正值(X*,Y*,Z*)。 [0037] 训练神经网络时，将陀螺仪的加速度传感器的加速度积分获得的相对坐标（X1,Y1,Z1）、陀螺仪角传感器积分获得的相对姿态角(θx,θy,θz)、陀螺仪地磁强度传感器的三轴强度（Bx,By,Bz）、水深传感器的深度数据h，基于卡尔曼滤波处理后获得的相对稳定的GPS两轴坐标(X2,Y2)、GPS两轴坐标在5s内标准差(σx,σy)值作为输入，预估实际坐标(X*,Y*,Z*)作为输出。 [0038] 通过模型航行实验纪录传感器数据，通过灵敏度分析筛选神经网络训练和测试用样本。将80%的样本作为训练集，10%作为测试集，10%作为验证集。 [0039] 使用PINN(Physics‑informed Neural Network)神经网络，基于Adam优化器，学习率选择0.01，神经网络有四层，包括两层隐藏层，每层神经元个数为20，激活函数选择ReLU函数（f（x）=max（0，x））PINN神经网络的学习过程由信号正向传播与误差反向传播两个过程组成。正向传 播时，输入样本从输入层传入，经各隐层逐层处理后，传向输出层，得到坐标预测值(X*’,Y’*,Z*’)，对时间进行求导，得到三轴速度分量预测值(U*’,V*’,W*’)，并计算方程与运动学方程的残差r。将坐标和残差分别通过交叉熵损失函数（L(y)=‑[ylogy’+(1‑y)log(1‑y’)]，y’为预测值，y为真实标签值）量化预测效果，并加权求总损失，进行反向传播，将输出层通过隐藏层向输入层逐层反转，并将误差分摊给各层的所有单元，从而获得各层单元的误差信号，再通过此误差信号修正个单元权值。 [0040] 神经网络的物理方程约束为质点运动的微分方程:max=X(t,x,y,z,vx,vy,vz) may=Y(t,x,y,z,vx,vy,vz) maz=Z(t,x,y,z,vx,vy,vz) 式中m为航行器质量，ax、ay、az为航行器加速度三轴分量，t为时间，x，y，z为航行器坐标，vx、vy、vz为航行器三轴速度，X、Y、Z为航行器三轴受力函数，该函数由对航行器的仿真计算求得。 [0041] 训练集分五批次训练，训练后用测试集测试是否精确，每次进行测试后认为单次结果误差小于5%为精确，每批次训练总体准确率在90%以上为精确。最后使用验证集判断网络鲁棒性，若仍然精确，则输出模型。 [0042] 神经网络的初始权重w为（‑1，1）间的随机数，初始偏置b为（‑1，1）间随机数。神经网络输出值y为：y=f（Σ（wxi+b）） f为激活函数，x表示输入，y表示输出。 [0043] 训练本神经网络的过程可以简化为如下步骤：赋予神经元权值随机数，设定loss函数； 将训练集分为五批次，在一个批次中，随机选取一个样本进行输入； 经过前向传播计算输出； 利用神经网络的输出输入的标签值、物理方程约束的残差，计算误差函数对输出 层各神经元的偏导数； 使用adam优化器，修正权值和偏置项； 循环将一个批次的结果输入，并将五个批次一次输入； 通过测试在每一个批次训练完成后输入神经网络判断准确率，未达标和重复训练 该批次； 五个批次的训练和测试完成后，用验证集再次验证模型准确度； 控制系统对本神经网络的装载和使用步骤如下： 训练完成后的网络会被装载到主控芯片中，作为坐标计算模型； 主控芯片将从控芯片A中的数据输入该模型，得到实际坐标，并用于路径点导航。 |
||||||
QQ群二维码
意见反馈
意见反馈