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

权利要求书

1.一种无人船二维激光雷达自稳装置，其特征在于：包括底座(1)、第一支架(2)、第一旋转轴(3)、第二支架(4)、第二旋转轴(5)、第三支架(6)和伺服控制系统，所述第一旋转轴(3)通过第一支架(2)安装在底座(1)上，所述第二旋转轴(5)通过第二支架(4)安装在第一旋转轴(3)上，所述第三支架(6)安装在第二旋转轴(5)上；所述底座(1)固定在无人船上，所述第三支架(6)用于安装二维激光雷达(7)；所述第一旋转轴(3)与第二旋转轴(5)在空间上相互垂直；

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

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

2.根据权利要求1所述的一种无人船二维激光雷达自稳装置，其特征在于：所述的第一旋转轴(3)与无人船的纵向轴线垂直。

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

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

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

B、双闭环PID控制

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

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

u(t)=kperror(t)+ki∫0terror(t)dt+kdderror(t)dt---(1)]]>

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

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

u(t)=kperror(t)+kiΣj=0terror(j)T+kderror(t)-error(t-1)T---(2)]]>

式中T为采样时间；

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

AnglePIDOut=kpe(t)+kiΣj=0te(j)T+kde(t)-e(t-1)T---(3)]]>

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

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

AngleRatePIDOut=kp′e′(t)+ki′Σj=0te′(j)T+kd′e′(t)-e′(t-1)T---(4)]]>

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

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