[发明专利]一种无人船二维激光雷达自稳装置及其控制方法

说明书

技术领域

本发明属于无人船领域，尤其涉及一种无人船二维激光雷达自稳装置及其控制方法。

背景技术

激光雷达是实现无人船环境感知的重要传感器，对于实现无人船自主导航避障、检测无人船航行环境的障碍物起着重要的作用。而二维激光雷达在一固定平面上，可以感知无人船360度的环境信息，将检测到的障碍物距离和角度数据发送到无人船的控制伺服控制系统。但是由于无人船在执行作业任务时，经常受到风浪等干扰使无人船产生摇摆。此时二维激光雷达易受到无人船摇摆产生的角度和角速度的影响，使得测量的数据不在预定平面上，导致无人船的控制伺服控制系统对障碍物误判。

发明内容

为解决现有技术存在的上述问题，本发明要设计一种能够消除无人船摇摆引起的倾斜对角度和角速度的影响，隔离各种扰动对激光雷达检测障碍物的影响，实现二维激光雷达随动位姿稳定控制的无人船二维激光雷达自稳装置及其控制方法。

为了实现上述目的，本发明的技术方案如下：一种无人船二维激光雷达自稳装置，包括底座、第一支架、第一旋转轴、第二支架、第二旋转轴、第三支架和伺服控制系统，所述第一旋转轴通过第一支架安装在底座上，所述第二旋转轴通过第二支架安装在第一旋转轴上，所述第三支架安装在第二旋转轴上；所述底座固定在无人船上，所述第三支架用于安装二维激光雷达；所述第一旋转轴与第二旋转轴在空间上相互垂直；

所述伺服控制系统包括伺服控制器、加速度计、陀螺仪、横滚伺服电机、俯仰伺服电机和光栅编码器；所述横滚伺服电机与第一旋转轴连接，所述俯仰伺服电机与第二旋转轴连接；所述光栅编码器有两个，分别与横滚伺服电机和俯仰伺服电机连接；所述伺服控制器、加速度计和陀螺仪安装在无人船上；所述伺服控制器通过导线分别与光栅编码器、加速度计和陀螺仪连接；

所述加速度计与陀螺仪提供无人船的姿态信息；所述伺服控制器接收来自加速度计和陀螺仪的无人船姿态信息，并且与光栅编码器传输来的伺服电机位置和速度信息进行对比，采用双闭环PID控制，实现二维激光雷达的自稳；所述光栅编码器有两个，分别检测横滚伺服电机和俯仰伺服电机的运行状态，所述运行状态包括角度和位置信息。

进一步地，所述的第一旋转轴与无人船的纵向轴线垂直。

一种无人船二维激光雷达自稳装置的控制方法，包括以下步骤：

A、获取无人船准确的横滚角和俯仰角信息

根据陀螺仪在动态时测量较准确但静态时性能较差、加速度计在静态时测量较准确但动态时性能较差的特点，通过卡尔曼滤波方法进行数据融合，得到无人船准确的横滚角和俯仰角信息。

B、双闭环PID控制

伺服控制器接收来自加速度计和陀螺仪的无人船姿态信息，并且与光栅编码器传输来的伺服电机位置和速度信息进行对比，采用双闭环PID控制，实现自稳。所述双闭环PID控制，外环是位置环，即角度环，内环是速度环，即角速度环。所述角度环，为控制的目标环，是控制所需要达到的目标，即补偿无人船摇摆所产生的横滚角/俯仰角。所述角速度环，是外环角度的导数，是为了提升伺服控制系统稳定性。角度环控制模式下伺服控制系统的动态响应速度较慢，在角度环控制的同时加入角速度控制，以改善扭矩的反应速度，使伺服控制系统快速平稳的工作，达到随动位姿稳定控制。

所述PID控制采用位置式PID控制：

式中u(t)为PID输出值，error(t)为期望值与实际值之差，k_p、k_i、k_d分别为比例、积分、微分的系数。

然后将积分量、微分量离散化得到PID计算公式：

式中T为采样时间。

根据公式(2)，角度环PID控制算法为：

式中，e(t)为加速度计、陀螺仪传输的无人船角度姿态信息和光栅编码器传输的伺服电机角度位置信息的偏差。

根据公式(2)，同理得角速度环PID控制算法：

式(4)中，e′(t)为加速度计、陀螺仪传输的无人船角速度姿态信息和微分伺服电机角度位置得到的角速度信息之间的偏差。

伺服控制器根据计算的AnglePIDOut和AngleRatePIDOut，转化为横滚伺服电机和俯仰伺服电机的控制量，输出到相应伺服电机以控制激光雷达的稳定。

与现有技术相比，本发明具有以下有益效果：