前些天突然接到个紧急的项目：某处需要实现对夜航船只进行追踪并用激光灯照射以保障夜航安全。这个项目紧急到什么程度呢？！现场激光灯都安装好了，还有三个星期就要验收了，但上家没搞定就甩给我们了:(
从技术上看，本项目没什么好写的，但正因为本项目如此之紧急，所以在工程上还有点意思，所以总结一下，以资参考。
系统组成
说起来，整个项目并不复杂。我完成的系统图如下：
其中：
一、现场采集器就是用之前在rust嵌入式开发之总结一文介绍过的我司标准板卡，主要完成：
- 到两激光灯的RS485连接，并以Pelco-D球机控制协议进行控制
- 和服务器的通信，实现远程控制
- 提供命令行接口，完成激光灯的坐标读取等工程测试、标定等基础工作
本还需要完成到AIS基站的RS232串口连接来实时获取AIS数据，但AIS基站已经安装完毕，上家却没和设备厂家说如何取数据的事，所以AIS厂家也就没留RS232接口给我们，好在AIS厂家还提供了TcpServer能力，所以从AIS基站读取AIS数据的功能移到了应用服务器处。
二、应用服务器就是我之前介绍过的docker版TMS系统，有全套的业务快速开发、数据采集与处理能力。包括了两块：
- 业务系统：java开发的tms业务后台，可以提供业务管控应用的低成本快速定制。本项目不需要向客户提供业务管控与应用，但需要制作几个测试操作界面，如模拟定位【即通过页面输入经纬度坐标，分别控制两灯的转动】、两灯的手动开关、开启关闭几个相关的debug能力以辅助调试、工程数据采集和验收
参考：jxTMS的设计思想
- 数据系统：python开发的数据采集、解析、保存、处理系统，提供了完整的数据处理框架，可以快速而低成本的实现数据的处理与分析
基于这两个系统，本项目一共只写了5个py文件，分别是：
1、site_ais_trace.py，在数据处理框架中定制一个站点。除站点的通用功能外【如设备管理、设备通信与远程控制等】，主要是提供激光灯的控制。代码非常简单，主要就是两个对象函数：
from module.ais_trace import ship, light, light_init
class site_ais_trace(site_packet):
def __init__(self, name):
#将激光灯的控制函数设置为本对象的control函数
light.set_control_func(self.control)
#继承父类
super(site_ais_trace,self).__init__('site_ais_trace', name)
#对激光灯进行初始化
light_init()
def control(self, slave=1, x=None, y=None, light=None):
#控制功能也非常简单，就是向现场采集器下达三个远程控制命令
#激光灯的控制是实现在现场控制器的laser_lamp_control函数中的
#该函数为命令行和远程控制所共用
rd = {'cmd':'laser_lamp_control'}
if not x is None:
#控制激光灯水平转动
#根据设备手册，指定水平转动的值
jxUtils.checkAssert(0 <= x and x <= 35999, 'x must in [0, 35999]')
#根据laser_lamp_control函数的要求来设置参数
rd['s'] = slave
rd['n'] = 'x'
rd['v'] = x
#将控制命令下达给本站对应的现场控制器
rs = self.systemCmd(rd)
jxGo.log('info', f'site_ais_trace::control[{slave}] x:{x} req:{rs}')
time.sleep(0.05)
if not y is None:
#控制激光灯俯仰转动
jxUtils.checkAssert(0 <= y and y <= 8999 or 27000 <= y and y <= 35999, 'y must in [0, 8999] or [27000, 35999]')
rd['s'] = slave
rd['n'] = 'y'
rd['v'] = y
rs = self.systemCmd(rd)
jxGo.log('info', f'site_ais_trace::control[{slave}] y:{y} req:{rs}')
time.sleep(0.05)
if not light is None:
#控制激光灯灯光的开启与关闭
jxUtils.checkAssert(light == 'close' or light == 'open', 'light must in open|close')
rd['s'] = slave
rd['n'] = 'light'
rd['v'] = light
rs = self.systemCmd(rd)
jxGo.log('info', f'site_ais_trace::control[{slave}] light:{light} req:{rs}')
time.sleep(0.05)
2、device_ais_trace.py，在数据处理框架中定制一个设备，用来从ais采集数据。主要就是设备的标准功能【如设备数据分析、保存、设备活跃性检测等】。代码同样非常简单，主要就是两个对象函数：
from module.tcpClient import tcpClient
from module.ais_trace import ship
#导入AIS解析
from ais.aismParser import aismParser
#创建一个AIS解析器
_aismParser = aismParser()
class device_ais_trace(device):
def __init__(self, name, mySite, conf):
#AIS设备的手动配置工作：不执行超时检查、每条数据都保存等
conf['timeOut'] = 0
conf['timeOutCheckInterval'] = 0
conf['saveDataInterval'] = 0
conf['dataType'] = 'aism'
#不将接收到的数据发送到数据总线上
conf['dataBusInform'] = False
super(device_ais_trace,self).__init__('device_ais_trace', name, mySite, conf)
#本设备的数据不是标准的获取方法【通过现场采集器采集并发回】，需要用tcpClient手动获取
self.client = tcpClient(self.recv, ip='xxx.xxx.xxx.xxx', port=pppp)
self.client.connect()
def recv(self, data):
#解析读取到的AIS数据
str = data.decode('utf-8')
l, rc = _aismParser.receive(str)
if rc:
for d in l:
#交ais_trace进行跟踪与照射的处理
rd = ship.check(d)
#如果需要照射，则保存接收到的AIS数据以备复查
#由于该点的AIS基站天线配置的较强大，会接收到周围数十公里的AIS数据
#该地是较大的港口，接收到的AIS数据量非常大，所以丢弃和本项目无关的AIS数据
if not rd is None:
d['light'] = rd
#设备的receive函数是数据处理框架的标准接收接口，会自动完成：
#设备状态检测、数据保存、数据广播【供其它感兴趣的应用使用】等工作
self.receive(d)
3、main.py，数据系统主入口函数，主要完成系统启动、系统装配等工作。具体代码为：
#命令行处理
sn, sa = jxUtils.init()
#站点配置【下述的站点配置和站点订阅，也可以用web界面操作完成】
sc = {}
sc['type'] = 'site_ais_trace'
sc['name'] = 'xxx_站点名_xxx'
#设备列表
dl = []
sc['devices'] = dl
#添加设备配置
#本项目就一个ais设备
d = {}
d['type'] = 'device_ais_trace'
d['name'] = 'xxx_设备名_xxx'
dl.append(d)
#将站点添加到系统中
site.addSite(sc)
#通过mqtt订阅该站点以实现和站点的数据收发
from jx.jxUtils import _mqttClient
_mqttClient.subscribe('xxx_站点名_xxx')
#启动一个pyService完成数据系统和业务系统的勾连，这样就可以通过业务系统中定制的web界面向数据系统下达命令了
jxUtils.startSlave(sn,sa)
注：一般用脚本来启动main.py，这样可以通过灵活的设置命令行参数来实现各种系统配置。本系统的启动脚本：
cd /home/tms/python/
python3 main_slave.py -n 'xxx_项目名' --startDeviceDataQuery --mqServerIP '127.0.0.1' --serviceName 'cwz01' --dataBus --dbName 'demoOrg_2255' --site --app --module --mqttServerIP '127.0.0.1' &
4、module/service_ais_trace.py，向标准的pyService处理框架中插入几个控制命令，实现业务系统对数据系统的命令，如在web界面直接下达开关灯命令：
from jx.mainService import mainService
from module.ais_trace import _light_1, _light_2
def lightControl(params):
jxGo.log('info',f'lightControl recv:{params}')
slave = params.get('slave', 1)
active = params.get('active', False)
if slave == 1:
if active:
_light_1.light_open()
else:
_light_1.light_close()
if slave == 2:
if active:
_light_2.light_open()
else:
_light_2.light_close()
mainService.register('lightControl',lightControl)
这样就实现了通过web界面进行灯光开关的控制功能，在调试阶段非常的方便，需要增加什么样的调试功能，就可以非常迅速的实现了。
5、module/ais_trace.py，是本项目专门开发的ais跟踪计算模块，是本项目的核心算法模块。后文再展开介绍其处理逻辑
三、AIS基站启动tcpServer来供应AIS数据；两激光灯【附带一个摄像头，两者安装在同一个云台上】则是受控设备，接受ais_trace计算出来的x轴、y轴坐标，然后执行相应的旋转与俯仰、开关灯等动作。
AIS跟踪的基本原理
通过AIS数据来追踪船只位置，原理非常简单：
1、用AIS解析器来解析船只发送的AIS数据，得到船只的经纬度数据
2、根据经纬度计算出船只到本站的距离，从xxxx米开始追踪，到警戒区即开始照射【白天不照射】，警戒区划定为yyy米
注：追踪、照射的两阶段接力处理算法，是原本考虑当AIS数据发送频率太低时，通过扩大追踪范围尽可能多的收集船只位置数据，然后拟合出船只的航迹，再通过船速计算出船只下一刻的可能位置进行补点，以实现较好的照射效果。但到现场后，由于需要保障的夜航距离非常近，这一补点算法没有实施的必要，所以取消。但跟踪、控制的两阶段算法已经没时间进行重构了，只能保留
3、当船只进入警戒区后，两灯分别根据船只经纬度计算船只到灯的水平面【x轴】的张角和垂直面【y轴】的俯仰角，然后将其映射到激光灯坐标系下，再换算成激光灯对应的控制数据，最后发送给现场控制器下达到两灯执行
4、由于该站为旅游项目，激光灯下方各处都存在人员行走和休息区域，考虑到强光对人眼的威胁，所以有必要对灯光的照射范围进行限制，当计算出的【x, y】超出许可范围时，不照射
所以，本项目的核心就是地球平面直角坐标系中的各种三角函数的运算。主要涉及：
- 经纬度转换
- 根据经纬度计算两点间距离
- 根据经纬度计算某点和本点的张角
- 坐标系旋转
- 不同坐标系的映射与变换
上述这些计算，只要大家初高中时三角函数学的还行，然后网上搜搜就完全可以写出来了。这里就不复赘述了。
有必要说明的只有一点，即：python的浮点数的精度问题。
python中常用的浮点数的精度在15-17位左右。而地球上经度一度差不多就是111公里，即1米就需要精确到百万分之九；地球半径我们取的是：6371004米，加上运算所需，所以浮点数的精度可能是不太够。
如果大家担心这一点，选择了Decimal，那就尽量设的大一点。
当然，就本项目来说，由于其它方面的误差太大，浮点数精度不够的影响可以忽略。
AIS跟踪的工程基准
本项目有4类7个基准数据，需要通过工程的方法加以确定。
这4类基准数据，是本项目保证追踪效果的基石：
- 本站基准经纬度：用来确定船只和本站的距离，超过一定距离的船只就不需要跟踪了。本数据可以在本站主要位置选点，精度要求也不需要太高，反正误差几米不过是将原本可以不跟踪的船也纳入跟踪而已，这点计算量不值一提
- 两灯的安装点经纬度：这是实现控制灯水平面旋转到船只所在方位的基础数据，自然是要求精度尽可能的高。但由于项目太紧急了，我的准备工作也自然不充分，没有借到差分的GPS设备。这种情况下经过和自己用手机打点的比较后，就选用了经过甲方确认的工程施工资料中的两灯经纬度【工程上的甩锅基本律：不准那是因为甲方提供的基准点数据的问题:)】
- 两灯的海拔高度：这是实现控制灯垂直面俯仰，使得灯对照船只的基础数据。确定起来也简单，就以甲方提供的工程资料中所标注的平台海拔加上灯头到平台的高度作为两灯的海拔高度就可以了
- 两灯安装后各灯的x轴零点朝向：这是完成计算后，将计算结果从经纬度坐标变换成激光灯的x轴坐标的基础数据。很遗憾，我们只有手机上的各种罗盘应用，这些应用都只给到度，简直郁闷至死！
其实，还有两灯的安装面的水平倾角这个基准数据。但原则上，我们肯定是要求水平安装的，而这在工程施工时也是最好矫正的。
但很遗憾，我看到激光灯的时候，某个灯的水平倾角肉眼可见的倾斜了，起码达到了十几度！而我是周一上去的，周五就要验收，还是台风天气，时不时的就风雨大作，大风天气下登高工作太过危险，所以干脆放弃了对两灯的安装面进行校准的想法。
这时，我们就面临两个关键的工程校准工作，这两者将直接决定项目成败：
- 两灯的x轴零点朝向如何校准？
- 两灯的水平倾角如何纠正？
既然手机罗盘只能达到度的精度，反正都已经是不尽如人意了，那干脆就用人眼视觉效果进行校准好了。
所以我的办法非常粗暴：
- 在平台上选择了几个非常醒目的标识点，打这些点的GPS经纬度
- 在不进行x轴校准的情况下计算出灯的x轴旋转度数
- 手动将摄像头旋转到标识点，一点一点的精细操作摄像头对正该标识点
- 从现场控制器通过命令行直接读取到此时激光灯的x轴数值
此时，直接读取到的x值和计算出的x值的差，就是激光灯x轴零点朝向的校准值。只要将几个标识点的校准值一一得到，就可以得到一个工程上比较满意的x轴零点朝向的校准值了。
但是，那个水平倾角误差较大的灯就会有很明显的误差：根据某点得到的x轴零点朝向的校准值，算出来的其它点的x值的误差最大的能达到25度！！这时看到的效果，就是船只没有被套住，即激光灯旋转过去后，船只不在画面内。
由于水平倾角无法归零，所以最终我选择了最重要的航线为基准剖面，以该面的x轴零点朝向的校准值作为最终值来设定激光灯的基准数据。
至于激光灯【x, y】值的范围约束，只要旋转激光灯，避开有人区域后，通过现场控制器的控制台读取此时的x、y值即可，然后根据一系列的【x, y】，取其中的最大最小值，就能确定【x, y】值的范围约束了。
AIS跟踪的程序逻辑
通过上面的讨论，实现AIS追踪的逻辑是比较简单的，只实现了两个类：ship和light，分别代表航经的船只、两个需要控制的激光灯。
ship主要是接收到AIS数据后完成：
- 计算船只到本站的距离，超过追踪距离的就丢弃
- 更新ship对象的实时信息【经纬度、距离、航速、驶向还是驶离、计算出的航迹等等】
- 请求两灯计算是否需要照射本船
- 当船只驶离本站后，经过一段时间后将其删除，以避免数据积累耗光内存
light主要是接收到船只的照射请求后完成：
- 检查是否需要照射，不在照射时间内就驳回请求
- 根据船只的经纬度计算船只到本灯的距离，超过警戒距离的驳回请求
- 根据经纬度计算船只到本灯的张角，并将计算结果变换到激光灯的水平坐标系
- 根据上述得到的本灯x轴零点朝向计算出纠正后的x值
- 如果x值超范围，则驳回请求
- 根据船只到本灯的距离计算出本灯到船只的俯仰角，然后换算成激光灯的y值
- 如果y值超范围，则驳回请求
- 根据计算出的【x, y】值下达照射指令
这里唯一需要额外考虑的就是：在航迹上最后一个允许照射点下达照射指令后，有可能就不再发出控制指令了，不可能让激光灯就这么一直照射着。所以，在下达照射指令后，必须启动一个防呆处理：如果过几分钟还没收到新的控制指令，就直接关灯。
结语
本项目从软件编写的角度来看非常简单，但由于工程上的各种坑，所以需要综合应用多种工程手段进行应对来保证照射效果。
正应了笔者反复说的那句话：程序员首先是一个工程师，工程师就是要解决问题的，而解决问题要靠我们经过长期训练所掌握的一整套的方法论。