[发明专利]一种基于航迹推算的移动机器人可见光定位方法

权利要求书

1.一种基于航迹推算的移动机器人可见光定位方法，其特征在于，包括以下步骤：

S1、使用传感器测量相邻的采样时间内的机器人的移动距离和航向角θ的变化，采用航迹推算定位算法推定机器人相对初始点A0的位置A₁,A₂,A₃,···,A_n,A_n+1；

S2、LED灯具传输一种代表灯具ID信息的高速脉冲编码调制通讯信号的可见光波形信号；

S3、使用光电检测器检测灯具高速脉冲编码调制通讯信号的频率,根据频率得出灯具ID，根据已知ID的灯具，进行VLC定位，使用三角定位法即可推算出机器人的位置；

S4、航迹推算经过一定时间后会产生累积误差，使用粒子滤波算法，将航迹推算和VLC定位的结果进行融合校正，从而将定位结果校准到新的更准确的点上。

2.如权利要求1所述的基于航迹推算的移动机器人可见光定位方法，其特征在于，所述步骤S1中的航迹推算定位算法如下：

设A_n(x_n,y_n),A_n+1(x_n+1,y_n+1),则有：

3.如权利要求1所述的基于航迹推算的移动机器人可见光定位方法，其特征在于，所述的步骤S2中的高速脉冲编码调制通讯信号的频率代表灯具ID信号，不同的频率代表着不同的ID。

4.如权利要求1所述的基于航迹推算的移动机器人可见光定位方法，其特征在于，所述步骤S3中灯具的ID与所在位置是已知的，测得灯具ID，采用三角定位法推算出所在位置，具体步骤如下：

S301、基于兰伯特模型,LED灯泡的信道增益可以表示为:

其中A是VLC定位模块中光电二极管探测器的物理面积,θ是对于接收轴的角度,φ是与LED灯泡有关的入射角度；m代表朗伯排放的顺序,并表示为：

其中φ1/2是LED灯泡的半功率角；

S302、设定LED发射器和定位模块接收器的光强度分别为I_t和I_r，如图二所示，具体公式如下：

其中C是常量，r是传感器与LED灯泡间的水平距离，d是传感器与LED灯泡之间的直线距离，h是传感器与LED灯泡之间的垂直距离；

S303、基于公式四可得知r、d和h之间的关系，具体公式如下：

(x_p-x)²+(y_p-y)²＝r_p² 公式六

S304、由上述接收器的位置可得知：

其中，(x_A，y_A),(x_B，y_B)，(x_C，y_C)分别代表LED_A，LED_B,LED_C的位置，r_A、r_B、r_C分别代表从LED_A，LED_B,LED_C到模块的距离，(x，y)为模块的位置。

5.如权利要求1所述的基于航迹推算的移动机器人可见光定位方法，其特征在于，所述步骤S4包括：

S401、所述误差的偏差范围用圆圈表示，时间越长，距离起始点越远，圆圈越大，误差范围越大；

S402、粒子的选取和初始化，设定粒子的集合为H＝{X^k|k＝1,2，,···，k}，k为粒子个数；系统状态空间取为X＝(x，y，v_x，v_y)^T,则粒子k的状态空间为X^k＝(x^k，y^k，v_x^k，v_y^k)^T，(x^k，y^k)为粒子k的位置，v_x^k和v_y^k分别为粒子k在x和y方向的速度，初始粒子的位置(x₀^k，y₀^k)(k＝1,2，···，k)，初始粒子的速度v_x^k，0和v_y^k，粒子的初始权重ω₀^k为1/k；

S403、机器人的运动模型和系统状态方程，由惯性导航模型可知，机器人在t时刻的位置坐标为：

当A_n+1与A_n时间间隔足够短，则V_n+1＝V_n，记由系统状态方程得：

S404、粒子重采样：当在k时刻从VLC定位获得一个新的观测数据后，需要对所有粒子的权重进行更新，并进行粒子重采样，依据粒子的，保留权重大的粒子，抛弃权重小的粒子，然后将粒子权值归一化：

粒子重采样采用随机重采样的方式，根据更新后的各粒子权重，将它们排列在[0，1]区间内，形成粒子权重区间，每个粒子的区间长度由其权重值决定；随机产生n个[0，1]区间内的数，找出其在权重区间的位置，将相应位置的粒子复制得到新的粒子集；由于权重大的粒子占据区间较长，被随机选中概率较大，权重小的粒子占据区间较短，被随机选中的概率较小，从而权重大的粒子被保留，权重小的粒子被舍弃；

S405、系统状态估计，根据重采样并更新之后的粒子的状态和权重，由系统状态空间Xⁿ＝(xⁿ，yⁿ，v_xⁿ，v_yⁿ)^T计算系统状态为：

A′_n+1(x′_n+1，y′_n+1)即为机器人当前位置，VLC校准航迹推算的定位时，将VLC定位的结果A′_n取代航迹推算的定位结果A_n，并作为下一次航迹推算的初始位置，以固定或者不固定的时间间隔循环，或者当误差范围超过一定阈值时触发循环条件，执行循环，定位结果即为A′₀，A′₁，A′₂，A′₃，···，A′_n，A′_n+1。