基于闭环PID的双推进器无人船自动驾驶横纵向控制算法制造技术

本发明专利技术涉及自动驾驶控制技术领域，尤其涉及基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，本发明专利技术采用GPS/IMU的位置、航向和速度信息，纵向控制方面，将组合导航的速度与临近点轨迹点的规划速度差值作为输入，采用位置PID算法计算双电机平均占空比；横向控制方面，计算船体航向与预瞄点的角度偏差作为输入，采用位置式PID算法实时计算左右电机的占空比的偏移值。根据横向平均占空比与纵向占空比的偏移值，计算左右电机的PWM输出量，并用串口控制左右电机转速以实现无人船稳定的速度控制与轨迹控制。从而使得无人船能够根据环境的变化及时调整左右电机功率，实现无人船航向和航速的调整，进而保证无人船始终沿着最佳航线航行。线航行。线航行。

【技术实现步骤摘要】
基于闭环PID的双推进器无人船自动驾驶横纵向控制算法


[0001]本专利技术涉及自动驾驶控制
，尤其涉及基于闭环PID的双推进器无人船自动驾驶横纵向控制算法。

技术介绍

[0002]无人船在水面自主航行时，会受到水流、风向等环境的影响，出现偏离预定轨迹的状态。为保持无人船沿着既定轨迹行驶，需要及时调整左右电机功率以修改船只航行方向和航行速度，从而减小无人船偏航角度，以保证无人船沿着最佳航线运行。目前国内外对于船舶偏航控制的策略是建立船舶动力学模型，并采用实验方法获取船舶回转及偏航特性参数，并据此对船舶进行控制，但该方法存在以下缺陷：(1)船舶干扰源较多，难以准确地建立运动学模型；(2)回转及偏航特性参数对外界环境较为敏感，测试得到的参数与真实行驶环境相比具有较大的误差

技术实现思路

[0003]为了解决上述至少一个技术问题，本专利技术提出一种基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，该方法融采用GPS/IMU的位置、航向和速度信息。纵向控制方面，将组合导航的速度与临近点轨迹点的规划速度差值作为输入，采用位置PID算法计算双电机平均占空比；横向控制方面，计算船体航向与预瞄点的角度偏差作为输入，采用位置式PID算法实时计算左右电机的占空比的偏移值。根据横向平均占空比与纵向占空比的偏移值，计算左右电机的PWM输出量，并用串口控制左右电机转速以实现无人船稳定的速度控制与轨迹控制。
[0004]为实现上述目的，本专利技术采用如下技术方案：
[0005]基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，包括：
[0006]S1，初始化误差、累计误差；
[0007]S2,读取无人船配备的组合导航，读取组合导航信息；
[0008]S3，计算临近点Node
nearest
并判断临近点是否为路径终点；
[0009]S4，若临近点不是路径终点，则采用位置式PID算法，以组合导航速度与临近点轨迹点的规划速度差值作为输入，计算均值占空比pwm
avek
；以船体航向与预瞄点的航向偏差作为输入，计算占空比偏移值pwm
biask
，从而实现无人船纵向和横向的控制；
[0010]S5，根据S4中均值占空比pwm
avek
和占空比偏移值pwm
biask
，计算左右电机占空比；
[0011]S6，硬件接口下发指令，控制无人船左右电机转速，已实现无人船稳定的速度控制与轨迹控制。
[0012]优选地，S2中组合导航输出的定位为WGS84坐标系下的经纬高，为方便计算，需要将WGS84坐标转换成局部坐标系下的东北天，计算公式如下：
[0013][0014]其中，为在东北天坐标系下的坐标；为转换矩阵；为所在定位点的地球卯酉圈曲率半径；(L,B,H)为经纬高；a为地球长半轴，为常量6378137.00；e为地球第一偏心率，为常量0.0066943799900965025；(L0,B0,H0)为局部坐标系原点的经纬高；N0为局部坐标系原点所在的地球卯酉圈曲率半径。
[0015]优选地，S2中无人船组合导航信息包括：实时位置信息、航向信息、速度信息，记为Position＝[lon.lat,alt,yaw,v]；其中位置信息为经度、纬度、高度，航向信息为船头与正北的夹角(0
‑
360
°
)，顺时针为正。
[0016]优选地，S3中临近点计算包括如下步骤：
[0017]S3.1,将定位信息(lon,lat,alt)转换成相对参考点的东北天(e,n,s)；
[0018]S3.2,对路径点Path＝{Node0,Node1,Node2,Node3,
…
Node
n
}中所有点，求与定位点的距离
[0019]S3.3，对S3.2中计算出的所有路径点进行排序，与定位点距离最小的点位为临近点。
[0020]优选地，S4中速度差值即为预瞄点Node
p
上期望速度与实际速度之差，公式如下：v
err
＝v
p
‑
v(2.2)，其中，V
err
为速度差值，V
p
为预瞄点上期望速度，V为实际速度。
[0021]优选地，所述预瞄点Node
p
计算包括如下步骤：
[0022]S101，初始化参考点为临近点Node
nearest
，初始化累积距离为0，即：Node
ref
＝Node
nearest
，D
sum
＝0；
[0023]S102，从临近点开始，求解该点Node
i
与参考点的距离D
i
，并更新参考点和累积距离D
sum
，即：Node
ref
＝Node
i
，D
sum
＝D
sum
+D
i
；
[0024]S103，对S102进行循环，当累计距离D
sum
≥D
p
停止循环，此时的点为预瞄点Node
p
。
[0025]优选地，S4中航向偏差计算包括如下步骤：
[0026]S201,根据组合导航信息输出航向角yaw，求解无人船航向向量为：
[0027]n＝[n0,n1]T
＝[sin(yaw),cos(yaw)]T
；
[0028]S202,根据预瞄点Node
p
的东向北向[e
p
,n
p
]、定位点的东向北向[e,n]，可求解定位点到预瞄点的单位向量：
[0029]t＝[t0,t1]T
＝[(e
p
‑
e)/d,(n
p
‑
n)/d]T
[0030]其中为欧式距离；
[0031]S203,航向偏差的求解为：
[0032]yaw
err
＝sign(t
×
n)t
·
n
ꢀꢀꢀ
(2.1)
[0033]其中sign为符号函数，t
×
n＝t0n1‑
t1n0为差乘，t
·
n＝t0n0+t1n1为点乘。
[0034]优选地，位置式PID公式如下：
[0035]式中：K
p
为比例系数；为积分系数，为积分误差；为微分系数，(e
k
‑
e
k
‑1)为微分误差；e
k
、e
k
‑1为第k次、第k
‑
1次的误差。
[0036]优选地，无人船横向和纵向，采用两套独立的PID参数，分别进行平均占本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，其特征在于，包括：S1，初始化误差、累计误差；S2,读取无人船配备的组合导航，读取组合导航信息；S3，计算临近点Node
nearest
并判断临近点是否为路径终点；S4，若临近点不是路径终点，则采用位置式PID算法，以组合导航速度与临近点轨迹点的规划速度差值作为输入，计算均值占空比pwm
avek
；以船体航向与预瞄点的航向偏差作为输入，计算占空比偏移值pwm
biask
，从而实现无人船纵向和横向的控制；S5，根据S4中均值占空比pwm
avek
和占空比偏移值pwm
biask
，计算左右电机占空比；S6，硬件接口下发指令，控制无人船左右电机转速，已实现无人船稳定的速度控制与轨迹控制。2.根据权利要求1所述基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，其特征在于，S2中组合导航输出的定位为WGS84坐标系下的经纬高，为方便计算，需要将WGS84坐标转换成局部坐标系下的东北天，计算公式如下：其中，为在东北天坐标系下的坐标；为转换矩阵；为所在定位点的地球卯酉圈曲率半径；(L,B,H)为经纬高；a为地球长半轴，为常量6378137.00；e为地球第一偏心率，为常量0.0066943799900965025；(L0,B0,H0)为局部坐标系原点的经纬高；N0为局部坐标系原点所在的地球卯酉圈曲率半径。3.根据权利要求1所述基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，其特征在于，S2中无人船组合导航信息包括：实时位置信息、航向信息、速度信息，记为Position＝[lon.lat,alt,yaw,v]；其中位置信息为经度、纬度、高度，航向信息为船头与正北的夹角(0
‑
360
°
)，顺时针为正。4.根据权利要求1所述基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，其特征在于，S3中临近点计算包括如下步骤：S3.1,将定位信息(lon,lat,alt)转换成相对参考点的东北天(e,n,s)；S3.2,对路径点Path＝{Node0,Node1,Node2,Node3,
…
Node
n
}中所有点，求与定位点的距离S3.3，对S3.2中计算出的所有路径点进行排序，与定位点距离最小的点位为临近点。5.根据权利要求1所述基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，其特征在于，S4中速度差值即为预瞄点Node
p
上期望速度与实际速度之差，公式如下：v
err
＝v
p
‑
v (2.2)，其中，Verr为速度差值，Vp为预瞄点上期望速度，V为实际速度。6.根据权利要求5所述基于闭环PID的双推进器无人船自动驾驶横纵向控制算法，其特征在于，所述预瞄点Node
p
计算包括如下步骤：
S101，初始化参考点为临近点Node
nearest
，初始化累积距离为0，即：Node
ref
＝Node
nearest
，D
...

【专利技术属性】
技术研发人员：张庶，张泽启，
申请(专利权)人：北京启奥人工智能科技有限公司，
类型：发明
国别省市：