[发明专利]融合图像传感器和惯性传感器的智能小车控制方法及系统

权利要求书

1.一种融合图像传感器和惯性传感器的智能小车控制方法，其特征在于，所述的控制方法包括以下步骤：

S1、惯性传感器获取智能小车在t时刻相对于自身坐标系的角速度信息以及加速度将角速度信息转换为智能小车相对于惯性坐标系的旋转矩阵ω，将加速度转换为智能小车相对于惯性坐标系的加速度

S2、图像传感器获取含有空间位置已知的标志点的图片，构建3条直线相交于标志点，图像传感器坐标系原点与上述3条直线分别构成3个平面，计算获得3个平面的法向量γ_i，i＝1,2,3；

S3、将所述的旋转矩阵ω和3个平面的法向量γ_i经过融合计算得到智能小车的旋转姿态；

S4、构造隐式观测方程，对上述隐式观测方程进行线性化以及离散化，然后输入到卡尔曼滤波器得到递推方程，输出智能小车在惯性坐标系的坐标位置；

S5、将智能小车的旋转姿态、智能小车在惯性坐标系的坐标位置和空间位置已知的标志点信息融合计算后输出舵机控制信号以及电机控制信号，分别控制小车的转向和速度。

2.根据权利要求1所述的融合图像传感器和惯性传感器的智能小车控制方法，其特征在于，所述的步骤S1过程如下：

S101、惯性传感器通过陀螺仪测得小车在自身坐标系的x、y、z方向上的角速度，将角速度分别乘以惯性传感器的上采样时间，获得其旋转角度ω₁，ω₂，ω₃；

S102、上述旋转角度ω₁，ω₂，ω₃经过下列转换计算得到智能小车相对于惯性坐标系的旋转矩阵ω

S103、惯性传感器通过加速度计得到智能小车在t时刻相对于自身坐标系的加速度加速度测量值含有白噪声ε(t)、以及常值偏移量δ，通过下式转换计算得到智能小车相对于惯性坐标系的加速度

其中，R为智能小车的状态矩阵，g^S为惯性坐标系的重力向量。

3.根据权利要求1所述的融合图像传感器和惯性传感器的智能小车控制方法，其特征在于，所述的步骤S2过程如下：

S201、图像传感器在在t时刻捕获到智能小车前方的图像，并且经过降噪、锐化处理后输入到标志点识别模块；

S202、所述的标志点识别模块如果检测不到标志点，则使智能小车进入自转巡检状态，直至检测到标志点，其中，所述的标志点是真实世界当中用于协助定位的任意物体；

S203、将经过标志点识别模块的图像输入到边缘检测模块，边缘检测模块输出至少1条相交于标志点的直线，如果边缘检测模块输出的相交于标志点的直线数目小于3，则选取图像的左下角和右下角与标志点的连线作为其中2条直线；如果边缘检测模块输出的相交于标志点的直线数目大于3，则随机返回3条相交于标志点的直线；

S204、利用斜率求取上述3条直线的方向向量然后任取直线上一点与图像传感器的位置坐标构成向量q^c，则图像传感器的位置坐标与上述3条直线形成的3个平面的法向量为：

其中，×表示向量的乘法运算，i＝1,2,3表示3个平面的编号。

4.根据权利要求1所述的融合图像传感器和惯性传感器的智能小车控制方法，其特征在于，所述的步骤S3过程如下：

S301、3个平面的法向量γ_i和状态矩阵R存在以下关系：

其中，为3条直线的方向向量，R^T表示矩阵R的转置，μ_i∈{-1,1}为方向参数，它的取值取决于γ_i的方向；

S302、智能小车的旋转状态矩阵利用智能小车相对于惯性坐标系的旋转矩阵ω以及状态矩阵R计算得到：