一种无人船二维激光雷达自稳装置的制作方法

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

背景技术：

激光雷达是实现无人船环境感知的重要传感器，对于实现无人船自主导航避障、检测无人船航行环境的障碍物起着重要的作用。而二维激光雷达在一固定平面上，可以感知无人船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，转化为横滚伺服电机和俯仰伺服电机的控制量，输出到相应伺服电机以控制激光雷达的稳定。

与现有技术相比，本实用新型具有以下有益效果：

1、本实用新型通过将自稳装置应用到无人船二维激光雷达上，实现了二维激光雷达测距在同一平面上，解决了因无人船摇摆导致误将水平面判定为障碍物的问题，隔离了各种干扰对二维激光雷达测距的影响，从而实现了二维激光雷达随动位姿稳定控制。

2、本实用新型通过卡尔曼滤波方法，将加速度计和陀螺仪输出的角度值进行融合，实现了无人船横滚角和俯仰角的准确获得。

3、本实用新型通过对伺服电机进行双闭环PID控制，实现了伺服控制系统动态响应的快速性，提高了伺服电机扭矩的反应速度。

附图说明

图1是激光雷达自稳装置正视图。

图2是激光雷达自稳装置左视图。

图3是激光雷达自稳装置俯视图。

图4是伺服控制系统结构图。

图5是伺服电机双闭环控制。

图中：1、底座，2、第一支架，3、第一旋转轴，4、第二支架，5、第二旋转轴，6、第三支架，7、二维激光雷达，8、横滚伺服电机，9、俯仰伺服电机。

具体实施方式

下面将结合本实用新型实施例中的附图，对本实用新型实施例中的技术方案进行清楚、完整地描述。

如图1-3所示，本实用新型的装置通过最下部的底座1固定在无人船上。底座1通过第一支架2与第一旋转轴3连接，第一旋转轴3由横滚伺服电机8控制旋转，用以补偿无人船因外界扰动产生的横滚角。横滚伺服电机8是一个用脉宽调制来产生精准角度位置的定位电机。输入横滚伺服电机8的PWM的占空比决定了横滚伺服电机8的角度值，并且占空比和横滚伺服电机8所处角度值成线性关系，根据线性比值，来将横滚伺服电机8定位到期望的角度。第一个旋转轴通过第二支架4与第二个旋转轴连接，第二旋转轴5由俯仰伺服电机9控制，用以补偿无人船因外界扰动产生的俯仰角。第二旋转轴5通过第三支架6与二维激光雷达7相连，二维激光雷达7固定在第三支架6的顶部，二维激光雷达7自身可以进行360度旋转，以检测无人船周围的障碍物。

本实用新型通过伺服控制系统实现无人船二维激光雷达7随动位姿稳定控制。如图4所示，所述伺服控制系统由伺服控制器、加速度计、陀螺仪、横滚伺服电机8、俯仰伺服电机9、光栅编码器组成。

所述加速度计与陀螺仪提供无人船的姿态信息。在无人船匀速航行过程中，重力加速度可以分解成x、y、z三个方向的分加速度。而加速度计可以测量某一时刻x、y、z三个方向的加速度值。利用各个方向的分量与重力加速度的比值可以得到无人船的横滚角和俯仰角。但是，在无人船加速航行过程中，加速度计获得的加速度包含船体的加速度，所以无人船在运动时用加速度计获得无人船的姿态信息会有一定的误差存在。此外，陀螺仪输出角速度信息，对角速度进行积分，即可获得无人船的姿态角度信息。但是通过多次累加，产生的角度误差也比较大。但是陀螺仪自身的优点是受加速度影响较小。所以本实用新型结合两种姿态传感器的特点，将产生的数据经过伺服控制器进行融合处理，得到无人船准确的姿态信息。

所述融合处理，就是通过卡尔曼滤波方法，得到最优角度的估计值。

设加速度计输出角度为陀螺仪输出角度为以及Δt时间内陀螺仪输出角度的变化量以上三个量作为伺服控制系统状态量，则伺服控制系统的状态方程和观测方程为：

上式中，V_a是加速度计的过程噪声，它的协方差为Q_a；V_g是陀螺仪的过程噪声，它的协方差为Q_g；Y(K)为观测信号，按照陀螺仪的权重1-m和加速度计的权重m相加得到。V_k为观测噪声，其协方差为R_k。

定义Δa为无人船加速运行时的估计算子：Δa＝|a-g_c|/g_c，其中a为无人船的加速度，g_c为重力加速度。无人船加速度越大，Δa值越小，m值越大，加速度计输出的角度可信度越小，其具体计算公式为：

伺服控制系统的预测协方差阵为：

P(k|k-1)＝AP(k-1|k-1)A^T+Q

伺服控制系统的增益方程为：

K(k)＝P(k|k-1)H^T(HP(k|k-1)H^T+R_k)^-1

其中：

H为观测矩阵。

根据增益，计算伺服控制系统的状态变量，并且更新协方差阵，在进行迭代运算，得到融合的测量角度X(k|k)：

最优角度的估计值为：

式中，和分别为陀螺仪和加速度计测得的无人船横滚角，和分别为陀螺仪和加速度计测得的无人船俯仰角，和分别为伺服控制系统观测无人船的横滚角和俯仰角。

所述光栅编码器可以输出自稳装置的角度位置信息，并且经过差分可以求得角速度信息。本实用新型中包含两个光栅编码器，分别检测横滚伺服电机8和俯仰伺服电机9的运行状态。

所述伺服控制器接收来自加速度计和陀螺仪的无人船姿态信息和光栅编码器关于伺服电机的角度位置信息和角速度信息，然后进行控制算法运算，输出PWM波信号给横滚伺服电机8和俯仰伺服电机9。

所述控制算法，是对横滚伺服电机8和俯仰伺服电机9分别进行双闭环PID控制。

双闭环PID控制，外环是位置环，即角度环，内环是速度环，即角速度环。所述角度环，为控制的目标环，是控制所需要达到的目标，即补偿无人船摇摆所产生的横滚角/俯仰角。所述角速度环，是外环角度的导数，是为了提升伺服控制系统稳定性。角度环控制模式下伺服控制系统的动态响应速度较慢，在角度环控制的同时加入角速度控制，以改善扭矩的反应速度，使伺服控制系统快速平稳的工作，达到随动位姿稳定控制。

如图5所示，以控制横滚伺服电机8为例，介绍双闭环PID控制方法。执行作业任务的无人船因受风、浪、流等外界干扰，产生摇摆。首先无人船的加速度和陀螺仪输出无人船的姿态信息，经过伺服控制器的融合，得到准确的无人船横滚角。此时需要伺服控制系统对二维激光雷达7进行伺服校正。所述伺服校正包含位置校正和速度校正。位置校正是双闭环PID控制的外环，功能为完成伺服控制算法对自稳装置横滚角位置校正。位置校正的输入为无人船的横滚角和光栅编码器采样得到的横滚伺服电机8的绝对角度值信息，根据横滚角和角度值信息进行位置校正。输出的数据作为速度校正的输入。其中，光栅编码器输出的横滚伺服电机8的角度信息，经伺服控制器进行微分运算，得到横滚伺服电机8的角速度信息，此信息作为速度环的反馈量。速度校正是双闭环PID控制的内环，功能为完成伺服控制算法对自稳装置横滚角速度的校正。速度校正的输入为位置校正的输出和微分运算得到的横滚伺服电机8的角速度，经过角速度PID控制，输出PWM波信号控制横滚伺服电机8到达随动位姿稳定控制。俯仰伺服电机9的控制与横滚伺服电机8的控制方法相同，两者相互独立。

本实用新型不局限于本实施例，任何在本实用新型披露的技术范围内的等同构思或者改变，均列为本实用新型的保护范围。