一种基于粒子优化的光电传感设备的位姿偏差估计方法与流程

阅读：评论：0

1.本发明涉及光电传感设备标定技术领域的位姿偏差估计方法，具体是一种基于粒子优化(particle swarm optimization，pso)的光电传感设备的位姿偏差估计方法。

背景技术：

2.利用可见光及红外成像设备，能够获取视野范围内的目标的视频及图像信息，从而实现识别、跟踪等任务，其常见的应用场景有无人机反制站等。然而，在光电传感设备的安装过程中，不可避免的出现与设定的安装位姿有偏差的情况，此偏差属于一种系统误差。此误差会影响传感器对目标的测量精度，因此，如何利用有限的资源，实现对此误差的高精度、高效率的估计，是部署及使用光电传感设备中具有重要意义的课题。目前，现有技术中尚没有能够实现该目标的技术。

技术实现要素：

3.有鉴于此，本发明提供一种基于粒子优化算法的光电传感设备位姿偏差估计方法，能够解决光电传感设备在安装时出现与预设位姿存在偏差的问题。
4.为了实现上述目的，本发明采用的技术方案为：
5.一种基于粒子优化的光电传感设备的位姿偏差估计方法，包括以下步骤：
6.步骤1，以光电传感设备的实测位置为原点建立理想的东北天坐标系；光电传感设备的真实位姿相对于理想的东北天坐标系的偏差包括位移偏差和角度偏差，其中，位移偏差为(δx,δy,δz)，δx,δy,δz分别表示x、y、z轴的位移偏差，角度偏差为(δθx,δθy,δθz)，δθx,δθy,δθz分别表示绕x、y、z轴的旋转角度；
7.步骤2，在光电传感设备的周围空间中设置多个特征点，测量各特征点的实测位置；通过光电传感设备对各特征点进行观测，得到光电传感设备观测到的各特征点的方位角与俯仰角；
8.步骤3，设计如下目标函数：
[0009][0010]
式中，n为特征点的数量；对于特征点i，为根据粒子中的一个粒子计算出来的光电传感设备相对于特征点i的方位角，θ
i1
为光电传感设备对特征点i观测到的方位角，为根据粒子中的一个粒子计算出来的光电传感设备相对于特征点i的俯仰角，θ
i2
为光
电传感设备对特征点i观测到的俯仰角；δ
azi
为位置测量设备在经纬度方向上的固有误差，δ
pit
为高程测量设备测量高程的误差；ri为特征点i与理想东北天坐标系原点的距离；δ
θ1
和δ
θ2
分别为光电传感设备测量方位角和俯仰角的固有误差；
[0011]
步骤4，设置种规模n、粒子维度d、迭代次数t、惯性权重w、学习因子c1,c2，以及粒子位置边界中的位移偏移量边界范围、角度偏移量边界范围、粒子速度边界范围；
[0012]
步骤5，按照粒子位置边界大小，利用随机函数，初始化粒子位置xj(xi,yi,zi,θ
x
,θy,θz)，j＝1,2,...,n，xi,yi,zi,θ
x
,θy,θz分别表示光电传感设备坐标的x、y、z轴的位移偏移量和角度偏移量；初始化粒子速度v(v1,v2,v3,v4,v5,v6)；初始化各个粒子的最优位置pj为各个粒子的初始位置，各个粒子的最优目标函数值pbestj为各个粒子的初始目标函数值；初始整个种的最优位置g为初始种中目标函数值最小的粒子，整个种的最优目标函数值gbest为初始种中目标函数的最小值；
[0013]
步骤6，按下式更新v和x，并更新pj、pbestj、g、gbest：
[0014]
v(k+1)＝
[0015]
w*v(k)+c1*rand(0,1)*(pbest(k)-x(k))+c2*rand(0,1)*(gbest(k)-x(k))
[0016]
x(k+1)＝x(k)+v(k+1)
[0017]
式中，rand(0,1)表示0-1之间均匀分布的随机数,k、k+1表示迭代次数；
[0018]
步骤7，判断迭代次数是否大于t，若不满足，返回步骤5，否则终止迭代，输出g，即为最终的位姿偏差估计值。
[0019]
本发明的有益效果在于：
[0020]
1、本发明方法中，预设传感器基座安装位置为三维空间中的某一点p1(x1,y1,z1)，以此点为原点建立一个理想的三维坐标系，从而定义了其理想的姿态。然而在实际安装过程中，难以避免地会将传感器安装在某一点p2(x2,y2,z2)，记位移偏差为(δx,δy,δz)，且其姿态相较于上述定义的理想坐标系也会出现偏差的情况，记角度偏差为(δθx,δθy,δθz)。本发明将这两个偏差统称为位姿偏差，如何尽可能的准确地估计出这六个变量即是本方法的主要目的。为达到此目的，本发明在光电传感器周围空间中按一定方式选取若干个特征点，因而可以获取到光电传感器对这些特征点的俯仰角和方位角的测量值(称之为测量值)，若假设光电传感器的位姿偏差量已知，则可以通过几何关系计算出特征点相对于光电传感器的方位角和俯仰角(称之为计算值)，则理论上计算值和测量值是近似相等的。根据这个原理，则可以利用最优化的思想，构建一个目标函数，将上述描述位姿偏差的六个量设为待求值，上述问题转换为了如何求出一组最优的解，使得目标函数值最小的问题。
[0021]
2、本发明在光电传感设备位姿校准时采用粒子算法，利用计算机运算，将光电设备位姿偏差估计的求解过程转换成类似鸟觅食行为中的个体间的协作与竞争，也是基于“种”和“进化”的概念，实现复杂空间最优解的搜索。
附图说明
[0022]
图1是通过仿真实现本发明方法的流程图。
[0023]
图2是本发明方法的仿真实验结果。
具体实施方式
[0024]
以下结合附图和具体实施方式对本发明做进一步的详细说明。
[0025]
一种基于粒子优化的光电传感设备的位姿偏差估计方法，包括以下步骤：
[0026]
步骤1，以光电传感设备的实测位置为原点建立理想的东北天坐标系；光电传感设备的真实位姿相对于理想的东北天坐标系的偏差包括位移偏差和角度偏差，其中，位移偏差为(δx,δy,δz)，δx,δy,δz分别表示x、y、z轴的位移偏差，角度偏差为(δθx,δθy,δθz)，δθx,δθy,δθz分别表示绕x、y、z轴的旋转角度；
[0027]
步骤2，在光电传感设备的周围空间中设置多个特征点，测量各特征点的实测位置；通过光电传感设备对各特征点进行观测，得到光电传感设备观测到的各特征点的方位角与俯仰角；
[0028]
步骤3，设计如下目标函数：
[0029][0030]
式中，n为特征点的数量；对于特征点i，为根据粒子中的一个粒子计算出来的光电传感设备相对于特征点i的方位角，θ
i1
为光电传感设备对特征点i观测到的方位角，为根据粒子中的一个粒子计算出来的光电传感设备相对于特征点i的俯仰角，θ
i2
为光电传感设备对特征点i观测到的俯仰角；δ
azi
为位置测量设备在经纬度方向上的固有误差，δ
pit
为高程测量设备测量高程的误差；ri为特征点i与理想东北天坐标系原点的距离；δ
θ1
和δ
θ2
分别为光电传感设备测量方位角和俯仰角的固有误差；
[0031]
步骤4，设置种规模n、粒子维度d、迭代次数t、惯性权重w、学习因子c1,c2，以及粒子位置边界中的位移偏移量边界范围、角度偏移量边界范围、粒子速度边界范围；
[0032]
步骤5，按照粒子位置边界大小，利用随机函数，初始化粒子位置xj(xi,yi,zi,θ
x
,θy,θz)，j＝1,2,...,n，xi,yi,zi,θ
x
,θy,θz分别表示光电传感设备坐标的x、y、z轴的位移偏移量和角度偏移量；初始化粒子速度v(v1,v2,v3,v4,v5,v6)；初始化各个粒子的最优位置pj为各个粒子的初始位置，各个粒子的最优目标函数值pbestj为各个粒子的初始目标函数值；初始整个种的最优位置g为初始种中目标函数值最小的粒子，整个种的最优目标函数值gbest为初始种中目标函数的最小值；
[0033]
步骤6，按下式更新v和x，并更新pj、pbestj、g、gbest：
[0034]
v(k+1)＝w*v(k)+c1*rand(0,1)*(pbest(k)-x(k))+c2*rand(0,1)*(gbest(k)-x(k))x(k+1)＝x(k)+v(k+1)
[0035]
式中，rand(0,1)表示0-1之间均匀分布的随机数,k、k+1表示迭代次数；
[0036]
步骤7，判断迭代次数是否大于t，若不满足，返回步骤5，否则终止迭代，输出g，即为最终的位姿偏差估计值。
[0037]
下面通过仿真的方式对本发明方法进行验证：
[0038]
如图1，一种基于粒子优化的光电传感设备的位姿偏差估计方法，该方法采用启发式算法中的粒子算法对光电传感器位姿偏差进行估计。具体步骤如下：
[0039]
步骤1，产生模拟数据。以光电传感器被观测到的所在位置为原点建立理想的东北天(enu)坐标系，在此理想坐标系下，按照一定的分布方式产生若干数量的特征点，将此时的特征点的位置作为特征点的真实位置。设光电传感器的真实位姿相对于上述理想坐标系的偏差为：
[0040]
位移偏差为：(δx,δy,δz)，δx,δy,δz分别表示x、y、z轴的位移偏差；
[0041]
角度偏差为(δθx,δθy,δθz)，δθx,δθy,δθz分别表示绕x,y,z轴的旋转角度，即分别为俯仰角pitch，横滚角roll，航向角yaw。
[0042]
步骤2，设gps对特征点进行测量的经纬度(转换至平面坐标系下的x、y轴)误差范围为gpserror_xy米，高程测量设备测量高程(转换至平面坐标系下的z轴)的误差范围为gpserror_z米。将特征点真实位置的xyz坐标分别加上测量设备在各维度上的的测量误差，作为对特征点的观测值(对应实际场景中通过gps和高程设备获取特征点的三维坐标的值)。本例中，gpserror_xy设置为5米，gpserror_z设置为10米，各误差在相应范围内随机产生。然后，根据预设的光电传感器位姿偏差将特征点转换到光电传感器的真实坐标系下，得到特征点在光电真实坐标系下的坐标，根据此新坐标，利用几何关系计算出各特征点与光电传感器之间的方位角与俯仰角，对此方位角与俯仰角加上光电传感器测角的固有误差(本例中设置为0.2度)，作为实际场景中光电传感器观察到的各特征点的方位角与俯仰角。
[0043]
步骤3，设计如下目标函数：
[0044][0045]
式中，n为特征点的数量。对于某个特征点，为根据粒子中的一个粒子计算出来的方位角，θ
i1
为光电传感器对该特征点测量出来的方位角，同理和θ
i2
分别为计算出的俯仰角和光电传感器测量出的俯仰角。δ
azi
为gps设备测量经纬度方向上的固有误差，δ
pit
为高程测量设备测量高程方向上的误差；ri为该特征点与原点的距离；δ
θ1
和δ
θ2
分别为光电传感器测量方位角和俯仰角的固有误差。
[0046]
步骤4，设置种规模为n＝100，粒子维度d＝6，迭代次数t＝200，惯性权重w＝0.729，学习因子c1＝c2＝1.494，粒子位置边界中的位移偏移量边界范围为(-10，10)，角度偏移量边界范围为(-2,2)，粒子速度边界范围为(-5,5)。
[0047]
步骤5，按照粒子位置边界大小，利用随机函数，初始化粒子位置xj(xi,yi,zi,θ
x
,θy,θz)，j＝1,2,...,n；xi,yi,zi,θ
x
,θy,θz分别表示光电传感器坐标的x、y、z轴的位移偏移量和角度偏移量；同理，初始化粒子速度v(v1,v2,v3,v4,v5,v6)。初始化个体最优位置pj和最优
目标函数值pbestj，令pj＝xj，pbestj＝初始种中粒子j的目标函数值。初始化种最优位置g和最优目标函数值gbest，g＝初始种中目标函数值最小者，gbest＝初始种中目标函数最小值。
[0048]
步骤6，按如下公式更新v和x，并更新pj、pbestj、g、gbest：
[0049]
v(k+1)＝w*v(k)+c1*rand(0,1)*(pbest(k)-x(k))+c2*rand(0,1)*(gbest(k)-x(k))x(k+1)＝x(k)+v(k+1)
[0050]
式中，rand(0,1)表示0-1之间均匀分布的随机数，k为迭代次数。
[0051]
步骤7，判断迭代次数是否大于t，若不满足，返回步骤5，否则终止算法，输出g,即为最优预测值。
[0052]
仿真实验结果如图2所示，其中设定位移偏差为[3,6,9]，角度偏差为[0.5,0.9,1.7]，分别实验统计以下四种不同的特征点分布方式时的估计效果：
[0053]
1、在xy平面以1000米为半径做圆，均匀取圆上的10个点，z轴范围为(-100,100)。此处称之为圆形均匀分布。
[0054]
2、在xy平面以1000米为半径做圆，均匀取圆10个点，然后对x、y值均加上(0,2000)范围内的随机数，最后设置z轴范围为(-100,100)。此处称之为环绕随机分布。
[0055]
3、在xy平面近似直线排布，近似等间距取10个点。x范围为(-1500,1500)，y值约为500，z值范围为(-100,100)。此处称之为直线分布。
[0056]
4、一二象限随机各布置5个点，共10个特征点，一象限x取值范围为(500,1000)，二象限x取值范围为(-1000，-500)，y取值范围为(500,1000)，z取值范围为(-100,100)。
[0057]
从实验结果中可以总结出如下两点：
[0058]
1、实验结果验证了本方法的有效性，圆形均匀分布、环绕随机分布、直线分布、一二象限随机分布均体现了较好的估计效果。
[0059]
2、特征点圆形均匀分布、环绕随机分布时效果最佳，其次是一二象限随机分布，特征点直线分布时最不利于估计。
[0060]
总之，本发明选取若干个特征点，并得到这些特征点相对于传感器的方位角与俯仰角；设计目标函数；设置种规模、粒子维度、迭代次数、惯性权重、学习因子、粒子位置和速度的边界范围等超参数；初始化种、粒子速度、个体最优位置和最优目标函数值、全局最优位置和最优目标函数值；迭代更新粒子速度和位置，并更新全局最优位置和最优目标函数值、个体最优位置和最优目标函数值；判断是否满足终止迭代条件，输出最优目标函数值对应的个体，即为光电传感设备的位姿偏差的最优估计值。
[0061]
本发明简单易行，可用于光电传感设备位姿的校准，具有良好的应用效果。

技术特征：

1.一种基于粒子优化的光电传感设备的位姿偏差估计方法，其特征在于，包括以下步骤：步骤1，以光电传感设备的实测位置为原点建立理想的东北天坐标系；光电传感设备的真实位姿相对于理想的东北天坐标系的偏差包括位移偏差和角度偏差，其中，位移偏差为(δx,δy,δz)，δx,δy,δz分别表示x、y、z轴的位移偏差，角度偏差为(δθx,δθy,δθz)，δθx,δθy,δθz分别表示绕x、y、z轴的旋转角度；步骤2，在光电传感设备的周围空间中设置多个特征点，测量各特征点的实测位置；通过光电传感设备对各特征点进行观测，得到光电传感设备观测到的各特征点的方位角与俯仰角；步骤3，设计如下目标函数：式中，n为特征点的数量；对于特征点i，为根据粒子中的一个粒子计算出来的光电传感设备相对于特征点i的方位角，θ
i1
为光电传感设备对特征点i观测到的方位角，为根据粒子中的一个粒子计算出来的光电传感设备相对于特征点i的俯仰角，θ
i2
为光电传感设备对特征点i观测到的俯仰角；δ
azi
为位置测量设备在经纬度方向上的固有误差，δ
pit
为高程测量设备测量高程的误差；r
i
为特征点i与理想东北天坐标系原点的距离；δ
θ1
和δ
θ2
分别为光电传感设备测量方位角和俯仰角的固有误差；步骤4，设置种规模n、粒子维度d、迭代次数t、惯性权重w、学习因子c1,c2，以及粒子位置边界中的位移偏移量边界范围、角度偏移量边界范围、粒子速度边界范围；步骤5，按照粒子位置边界大小，利用随机函数，初始化粒子位置x
j
(x
i
,y
i
,z
i
,θ
x
,θ
y
,θ
z
)，j＝1,2,...,n，x
i
,y
i
,z
i
,θ
x
,θ
y
,θ
z
分别表示光电传感设备坐标的x、y、z轴的位移偏移量和角度偏移量；初始化粒子速度v(v1,v2,v3,v4,v5,v6)；初始化各个粒子的最优位置p
j
为各个粒子的初始位置，各个粒子的最优目标函数值pbest
j
为各个粒子的初始目标函数值；初始整个种的最优位置g为初始种中目标函数值最小的粒子，整个种的最优目标函数值gbest为初始种中目标函数的最小值；步骤6，按下式更新v和x，并更新p
j
、pbest
j
、g、gbest：v(k+1)＝w*v(k)+c1*rand(0,1)*(pbest(k)-x(k))+c2*rand(0,1)*(gbest(k)-x(k))x(k+1)＝x(k)+v(k+1)式中，rand(0,1)表示0-1之间均匀分布的随机数,k、k+1表示迭代次数；步骤7，判断迭代次数是否大于t，若不满足，返回步骤5，否则终止迭代，输出g，即为最
终的位姿偏差估计值。

技术总结

本发明公开了一种基于粒子优化的光电传感设备的位姿偏差估计方法，涉及监视和安防管控领域。该方法包括：选取若干个特征点，并得到这些特征点相对于传感器的方位角与俯仰角；设计目标函数；设置种规模、粒子维度、迭代次数、惯性权重、学习因子、粒子位置和速度的边界范围等超参数；初始化种、粒子速度、个体最优位置和最优目标函数值、全局最优位置和最优目标函数值；迭代更新粒子速度和位置，并更新全局最优位置和最优目标函数值、个体最优位置和最优目标函数值；判断是否满足终止迭代条件，输出最优目标函数值对应的个体，即为光电传感设备的位姿偏差的最优估计值。本方法可用于光电传感设备位姿的校准。电传感设备位姿的校准。电传感设备位姿的校准。