本发明公开了一种融合图像传感器和惯性传感器的智能小车控制方法及系统，该系统包括小车机械结构、接收子系统、传输控制子系统和驱动子系统。该系统使用图像传感器和惯性传感器作为信息输入。通过卡尔曼滤波器融合图像传感器获得的标志点的位置信息以及惯性传感器获得的小车的角速度、加速度，进而控制单片机动态输出PWM控制舵机和电机，使得智能小车准确到达给定标志点。本发明将图像传感器和惯性传感器的信息相融合，加快了位姿的反馈速度，实现了对智能小车精确且高速的控制。
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方向上的角速度，将角速度分别乘以惯性传感器的上采样时间，获得其旋转角度ω1，ω2，ω3；
S102、上述旋转角度ω1，ω2，ω3经过下列转换计算得到智能小车相对于惯性坐标系的旋转矩阵ωS103、惯性传感器通过加速度计得到智能小车在t时刻相对于自身坐标系的加速度加速度测量值 含有白噪声ε(t)、以及常值偏移量δ，通过下式转换计算得到智能小车相对于惯性坐标系的加速度其中，R为智能小车的状态矩阵，gS为惯性坐标系的重力向量。
3.根据权利要求1所述的融合图像传感器和惯性传感器的智能小车控制方法，其特征在于，所述的步骤S2过程如下：S201、图像传感器在在t时刻捕获到智能小车前方的图像，并且经过降噪、锐化处理后输入到标志点识别模块；
S202、所述的标志点识别模块如果检测不到标志点，则使智能小车进入自转巡检状态，直至检测到标志点，其中，所述的标志点是真实世界当中用于协助定位的任意物体；
S203、将经过标志点识别模块的图像输入到边缘检测模块，边缘检测模块输出至少1条相交于标志点的直线，如果边缘检测模块输出的相交于标志点的直线数目小于3，则选取图像的左下角和右下角与标志点的连线作为其中2条直线；如果边缘检测模块输出的相交于标志点的直线数目大于3，则随机返回3条相交于标志点的直线；
S204、利用斜率求取上述3条直线的方向向量 然后任取直线上一点与图像传感器的位置坐标构成向量qc，则图像传感器的位置坐标与上述3条直线形成的3个平面的法向量为：其中，×表示向量的乘法运算，i＝1,2,3表示3个平面的编号。
4.根据权利要求1所述的融合图像传感器和惯性传感器的智能小车控制方法，其特征在于，所述的步骤S3过程如下：S301、3个平面的法向量γi和状态矩阵R存在以下关系：
其中， 为3条直线的方向向量，RT表示矩阵R的转置，μi∈{-1,1}为方向参数，它的取值取决于γi的方向；
S302、智能小车的旋转状态矩阵 利用智能小车相对于惯性坐标系的旋转矩阵ω以及状态矩阵R计算得到：
5.根据权利要求1所述的融合图像传感器和惯性传感器的智能小车控制方法，其特征在于，所述的步骤S4过程如下：S401、已知标志点坐标为βs，设智能小车的测量坐标为M1＝ps(t)，令智能小车的真实坐标Y1＝(y1,y2,y3)，由于Y1为真实值，则其和测量值M1有以下关系：M1＝Y1+V1
S402、记x(t)＝[(ps(t)-βs)Tvs(t)T(-β)T]T，并得出如下方程：其中，W(t)＝-RTε(t)，
其中，ε(t)为加速度测量值 含有的白噪声，R为状态矩阵， 为R的共轭矩阵，RT表示矩阵R的转置，I3为标准对角矩阵，M1为智能小车的测量坐标，Y1为智能小车的真实坐标，V1为测量噪声；
其中k表示离散化的时间，Φ表示状态转移矩阵，G表示输入增益矩阵，Z为观测值，为系统状态，HZ和VZ为系数矩阵，构造卡尔曼预测方程如下
其中P为卡尔曼估计误差协方差矩阵，K表示卡尔曼增益，Qk＝var(W(k))为建模误差协T方差矩阵，C、D为系数矩阵，Rk+1＝D*var(vec(V))*D 为测量误差的协方差矩阵，Θ为修正量；
S404、由上述滤波更新方程，计算出智能小车在惯性坐标系的观测坐标ps。
6.根据权利要求1所述的融合图像传感器和惯性传感器的智能小车控制方法，其特征在于，所述的步骤S5过程如下：S501、根据智能小车的旋转姿态、智能小车在惯性坐标系的坐标位置、以及标志点坐标βs，由几何关系计算出智能小车的转弯半径；
S502、将智能小车的转弯半径作为控制量控制单片机分别输出相应占空比的PWM信号到舵机驱动电路以及电机驱动电路；
S503、舵机驱动电路根据接收的PWM信号控制舵机转动，电机驱动电路根据接收的PWM信号控制电机H桥的IGBT元件通断，实现对小车前轮转向以及速度的控制。
7.一种融合图像传感器和惯性传感器的智能小车控制系统，其特征在于，所述的控制系统包括小车机械结构、接收子系统、传输控制子系统和驱动子系统，所述的小车机械结构为四轮小车，包括两个前轮和两个后轮；
所述的接收子系统包括图像传感器和惯性传感器，其中，图像传感器安装在智能小车的正前方，惯性传感器安装在与智能小车的底板平行的小车车体上；
所述的传输控制子系统包括ADC模数转换器以及单片机，上述图像传感器和惯性传感器都分别通过ADC模数转换器与单片机相连接；
所述的驱动子系统包括直流稳压模块、舵机驱动电路、舵机、电机驱动电路和直流电机，其中，所述的直流稳压模块分别给单片机、舵机驱动电路、电机驱动电路供电，所述的舵机与四轮小车的两个前轮相连，所述的直流电机与四轮小车的两个后轮相连，所述的舵机驱动电路与舵机相连，接收来自单片机的第一PWM输出口的控制信号，所述的电机驱动电路与直流电机相连，接收来自单片机的第二PWM输出口的控制信号。
8.根据权利要求7所述的融合图像传感器和惯性传感器的智能小车控制系统，其特征在于，所述的电机驱动电路含有单级式H型PWM变换器来控制直流电机的电压。
9.根据权利要求7所述的融合图像传感器和惯性传感器的智能小车控制系统，其特征在于，所述的舵机控制两个前轮的转角，所述的直流电机经过齿轮带动两个后轮的转动。
10.根据权利要求7所述的融合图像传感器和惯性传感器的智能小车控制系统，其特征在于，所述的图像传感器采用CMOS摄像头，所述的惯性传感器采用MEMS陀螺仪和加速度计。
融合图像传感器和惯性传感器的智能小车控制方法及系统
技术领域
[0001] 本发明涉及图像以及惯性传感器定位技术、小车控制技术领域，特别涉及一种融合图像传感器和惯性传感器的智能小车控制方法及系统。
背景技术
[0002] 惯性测量单元通常写作IMU，它的主要组成部分包括线加速度计、陀螺仪及信号转换器，其中，线加速度计主要负责测量在三个坐标轴方向上的线加速度，由此可以积分求得机器人在三个轴向上的线速度，进一步积分便可求得移动机器人的位置信息；而陀螺仪主要负责测量移动机器人的欧拉角或者角速度，由此可以求得机器人的姿态信息。综合线加速度计和陀螺仪的测量信息，便可以得到移动机器人的位置、姿态等运动参数信息，从而实现移动机器人的自主定位。但是，这种定位方法需要对线加速度计和陀螺仪的测量信号进行积分运算，时间越长，其积分的误差便会变得越大，造成定位结果的误差也会越来越大。同时，陀螺仪的测量信号容易受到电磁信号的干扰，数据的漂移情况比较严重，这一点也会影响其定位精度。
[0003] 而视觉定位方法主要是通过视觉相机对周围环境进行测量，获取图像数据，并对采集到的图像数据进行处理运算后得到移动机器人的运动参数，实现移动机器人的自主导航和定位。现在机器人领域应用的较为广泛的几种视觉定位技术包括单目相机定位和双目相机定位。单目相机定位即通过一个摄像头对外界环境的信息进行采集，因为无法获取周围环境的深度信息，所以在使用过程中必须从多个角度来采取同一个物体的图像，造成其应用过程比较繁琐。双目视觉定位方法中使用两个摄像头来采集外界环境信息，其使用的原理是模拟人的双眼对外界环境的观察方式，通过计算左右图像中的特征点变化值来确定相机自身的运动情况进而获得移动机器人的运动信息，实现机器人的定位和导航。双目相机采集得到的信息量较单目相机要大得多，定位结果更为准确。但同时由于系统采集的数据量较大，造成视觉算法的计算量也会很大，大大延长了计算的时间。
发明内容
[0004] 本发明的目的是为了解决现有技术中的上述缺陷，提供一种融合图像传感器和惯性传感器的智能小车控制方法及其系统。
[0005] 根据公开的实施例，本发明的第一方面公开了一种融合图像传感器和惯性传感器的智能小车控制方法，所述的智能小车控制方法包括以下步骤：
[0006] S1、惯性传感器获取智能小车在t时刻相对于自身坐标系的角速度信息以及加速度 将角速度信息转换为智能小车相对于惯性坐标系的旋转矩阵ω，将加速度 转换为智能小车相对于惯性坐标系的加速度
[0007] S2、图像传感器获取含有空间位置已知的标志点的图片，构建3条直线相交于标志点，图像传感器坐标系原点与上述3条直线分别构成3个平面，计算获得3个平面的法向量γi，i＝1,2,3；
[0008] S3、将所述的旋转矩阵ω和3个平面的法向量γi经过融合计算得到智能小车的旋转姿态；
[0009] S4、构造隐式观测方程，对上述隐式观测方程进行线性化以及离散化，然后输入到卡尔曼滤波器得到递推方程，输出智能小车在惯性坐标系的坐标位置；
[0010] S5、将智能小车的旋转姿态、智能小车在惯性坐标系的坐标位置和空间位置已知的标志点信息融合计算后输出舵机控制信号以及电机控制信号，分别控制小车的转向和速度。
[0011] 进一步地，所述的步骤S1过程如下：
[0012] S101、惯性传感器通过陀螺仪测得小车在自身坐标系的x、y、z方向上的角速度，将角速度分别乘以惯性传感器的上采样时间，获得其旋转角度ω1，ω2，ω3；
[0013] S102、上述旋转角度ω1，ω2，ω3经过下列转换计算得到智能小车相对于惯性坐标系的旋转矩阵ω
[0014]
[0015] S103、惯性传感器通过加速度计得到智能小车在t时刻相对于自身坐标系的加速度 加速度测量值 含有白噪声ε(t)、以及常值偏移量δ，通过下式转换计算得到智能小车相对于惯性坐标系的加速度
[0016]
[0017] 其中，R为智能小车的状态矩阵，gS为惯性坐标系的重力向量。
[0018] 进一步地，所述的步骤S2过程如下：
[0019] S201、图像传感器在在t时刻捕获到智能小车前方的图像，并且经过降噪、锐化处理后输入到标志点识别模块；
[0020] S202、所述的标志点识别模块如果检测不到标志点，则使智能小车进入自转巡检状态，直至检测到标志点，其中，所述的标志点是真实世界当中用于协助定位的任意物体；
[0021] S203、将经过标志点识别模块的图像输入到边缘检测模块，边缘检测模块输出至少1条相交于标志点的直线，如果边缘检测模块输出的相交于标志点的直线数目小于3，则选取图像的左下角和右下角与标志点的连线作为其中2条直线；如果边缘检测模块输出的相交于标志点的直线数目大于3，则随机返回3条相交于标志点的直线；
[0022] S204、利用斜率求取上述3条直线的方向向量 然后任取直线上一点与图像传感器的位置坐标构成向量qc，则图像传感器的位置坐标与上述3条直线形成的3个平面的法向量为：
[0023]
[0024] 其中，×表示向量的乘法运算，i＝1,2,3表示3个平面的编号。
[0025] 进一步地，所述的步骤S3过程如下：
[0026] S301、3个平面的法向量γi和状态矩阵R存在以下关系：
[0027]
[0028] 其中， 为3条直线的方向向量，RT表示矩阵R的转置，μi∈{-1,1}为方向参数，它的取值取决于γi的方向；
[0029] S302、智能小车的旋转状态矩阵 利用智能小车相对于惯性坐标系的旋转矩阵ω以及状态矩阵R计算得到：
[0030]
[0031] 进一步地，所述的步骤S4过程如下：
[0032] S401、已知标志点坐标为βs，设智能小车的测量坐标为M1＝ps(t)，令智能小车的真实坐标Y1＝(y1,y2,y3)，由于Y1为真实值，则其和测量值M1有以下关系：
[0033] M1＝Y1+V1
[0034] 其中V1为测量噪声；
[0035] S402、记x(t)＝[(ps(t)-βs)Tvs(t)T(-β)T]T，并得出如下方程：
[0036]
[0037] 其 中 ，
[0038] 其中，ε(t)为加速度测量值 含有的白噪声，R为状态矩阵， 为R的共轭矩阵，RT表示矩阵R的转置，I3为标准对角矩阵，M1为智能小车的测量坐标，Y1为智能小车的真实坐标，V1为测量噪声；
[0039] S403、对步骤S402中的方程进行离散化，有
[0040]
[0041] 以及在点 处线性化，构造观测方程
[0042] Z＝HZxz+VZ
[0043] 其中k表示离散化的时间，Φ表示状态转移矩阵，G表示输入增益矩阵，Z为观测值，为系统状态，HZ和VZ为系数矩阵，
[0044] 构造卡尔曼预测方程如下
[0045]
[0046] 其滤波更新方程如下
[0047]
[0048] 其中P为卡尔曼估计误差协方差矩阵，K表示卡尔曼增益，Qk＝var(W(k))为建模误差协方差矩阵，C、D为系数矩阵，Rk+1＝D*var(vec(V))*DT为测量误差的协方差矩阵，Θ为修正量；
[0049] S404、由上述滤波更新方程，计算出智能小车在惯性坐标系的观测坐标ps。
[0050] 进一步地，所述的步骤S5过程如下：
[0051] S501、根据智能小车的旋转姿态、智能小车在惯性坐标系的坐标位置、以及标志点坐标βs，由几何关系计算出智能小车的转弯半径；
[0052] S502、将智能小车的转弯半径作为控制量控制单片机分别输出相应占空比的PWM信号到舵机驱动电路以及电机驱动电路；
[0053] S503、舵机驱动电路根据接收的PWM信号控制舵机转动，电机驱动电路根据接收的PWM信号控制电机H桥的IGBT元件通断，实现对小车前轮转向以及速度的控制。
[0054] 根据公开的实施例，本发明的第二方面公开了一种融合图像传感器和惯性传感器的智能小车控制系统，所述的控制系统包括小车机械结构、接收子系统、传输控制子系统和驱动子系统，
[0055] 所述的小车机械结构为四轮小车，包括两个前轮和两个后轮；
[0056] 所述的接收子系统包括图像传感器和惯性传感器，其中，图像传感器安装在智能小车的正前方，惯性传感器安装在与智能小车的底板平行的小车车体上；
[0057] 所述的传输控制子系统包括ADC模数转换器以及单片机，上述图像传感器和惯性传感器都分别通过ADC模数转换器与单片机相连接；
[0058] 所述的驱动子系统包括直流稳压模块、舵机驱动电路、舵机、电机驱动电路和直流电机，其中，所述的直流稳压模块分别给单片机、舵机驱动电路、电机驱动电路供电，所述的舵机与四轮小车的两个前轮相连，所述的直流电机与四轮小车的两个后轮相连，所述的舵机驱动电路与舵机相连，接收来自单片机的第一PWM输出口的控制信号，所述的电机驱动电路与直流电机相连，接收来自单片机的第二PWM输出口的控制信号。
[0059] 进一步地，所述的电机驱动电路含有单级式H型PWM变换器来控制直流电机的电压。
[0060] 进一步地，所述的舵机控制两个前轮的转角，所述的直流电机经过齿轮带动两个后轮的转动。
[0061] 进一步地，所述的图像传感器采用CMOS摄像头，所述的惯性传感器采用MEMS陀螺仪和加速度计。
[0062] 本发明相对于现有技术具有如下的优点及效果：
[0063] 1)本发明创新性地把图像传感器和惯性传感器相融合对智能小车进行控制，即使使用价格低廉的惯性传感器，也能得到很高的控制精度。
[0064] 2)本发明克服了传统仅使用惯性传感器定位技术所带来误差累积问题。
[0065] 3)本发明克服了传统仅使用单目摄像头定位技术所带来的无法确定深度以及运算时间较长等问题，提高了系统的动态响应速度。
附图说明
[0066] 图1是本发明中融合图像传感器和惯性传感器的智能小车控制方法的流程图；
[0067] 图2是本发明中融合图像传感器和惯性传感器的智能小车控制系统的连接关系图；
[0068] 图3是本发明中直线投影到图像平面示意图；
[0069] 图4是本发明中卡尔曼滤波器设计过程示意图；
[0070] 图5是本发明中小车定位控制示意图。
具体实施方式
[0071] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0072] 实施例一
[0073] 如图1，本实施例公开了一种融合图像传感器和惯性传感器的智能小车控制方法，所述的智能小车控制方法包括以下步骤：
[0074] S1、惯性传感器获取智能小车在t时刻相对于自身坐标系的角速度信息以及加速度 所述角速度信息转换为智能小车相对于惯性坐标系的旋转矩阵ω，所述加速度转换为智能小车相对于惯性坐标系的加速度
[0075] 其中，惯性传感器不构成对本发明技术方案的限制，其它可获得角速度信息以及加速度的传感器亦可以作为本发明技术方案的替换技术手段。
[0076] 在具体实施方式中，该步骤S1包括以下步骤：
[0077] S101、所述惯性传感器通过陀螺仪测得小车在自身坐标系的x、y、z这3个方向上的角速度，将角速度分别乘所述惯性传感器的上采样时间，获得其旋转角度ω1，ω2，ω3；
[0078] S102、所述旋转角度ω1，ω2，ω3经过下列转换计算得到所述智能小车相对于惯性坐标系的旋转矩阵ω
[0079]
[0080] S103、所述惯性传感器通过加速度计得到智能小车在t时刻相对于自身坐标系的加速度 所述加速度测量值 含有白噪声ε(t),以及常值偏移量δ。通过下式转换计算得到智能小车相对于惯性坐标系的加速度
[0081]
[0082] 其中，R为智能小车的状态矩阵，gS为惯性坐标系的重力向量。
[0083] S2、图像传感器获取含有空间位置已知的标志点的图片，构建3条直线相交于所述标志点，图像传感器坐标系原点与所述3条直线分别构成3个平面，计算获得3个平面的法向量γi(i＝1,2,3)；
[0084] 在具体实施方式中，该步骤S2包括以下步骤：
[0085] S201、所述图像传感器在在t时刻捕获到智能小车前方的图像，并且经过降噪、锐化处理后输入到标志点识别模块；
[0086] S202、所述标志点识别模块如果检测不到标志点，则使小车进入自转巡检状态，直至检测到标志点；
[0087] 其中，所述标志点可以是真实世界当中的任意物体，例如墙角、人的脚、桌脚等可以协助定位的点。
[0088] S203、所述经过标志点识别模块的图像输入到边缘检测模块，所述边缘检测模块输出至少1条相交于标志点的直线，如果边缘检测模块输出的相交于标志点的直线数目小于3，则选取图像的左下角和右下角与标志点的连线作为其中2条直线；如果边缘检测模块输出的相交于标志点的直线数目大于3，则随机返回3条相交于标志点的直线；
[0089] S204、利用斜率求取步骤S203所述的3条直线的方向向量 然后任取直线上一点与图像传感器的位置坐标构成向量qc,则图像传感器的位置坐标与所述的3条直线形成的3个平面的法向量为：
[0090]
[0091] 其中，×表示向量的乘法运算，i＝1,2,3表示3个平面的编号。
[0092] S3、所述步骤S1获得的旋转矩阵ω和所述步骤S2获得的3个平面的法向量γ经过融合计算得到智能小车的旋转姿态；
[0093] 在具体实施方式中，该步骤S3包括以下步骤：
[0094] S301、所述步骤2获得的3个平面的法向量γi(i＝1,2,3)和状态矩阵R存在以下关系：
[0095]
[0096] 其中， 为步骤S203所述的3条直线的方向向量，RT表示矩阵R的转置；μi∈{-1,1}为方向参数，它的取值取决于γi的方向；
[0097] S302、智能小车的旋转状态矩阵 可以利用智能小车相对于惯性坐标系的旋转矩阵ω以及状态矩阵R计算得到：
[0098]
[0099] S4、构造隐式观测方程，对所述隐式观测方程进行线性化以及离散化，然后输入到卡尔曼滤波器得到递推方程，输出智能小车在惯性坐标系的坐标位置；
[0100] 在具体实施方式中，该步骤S4包括以下步骤：
[0101] S401、已知标志点坐标为βs，设小车的测量坐标为M1＝ps(t)，那么可以令小车的真实坐标Y1＝(y1,y2,y3)，由于Y1为真实值，则其和测量值M1有以下关系：
[0102] M1＝Y1+V1
[0103] 其中V1为测量噪声。
[0104] S402、记x(t)＝[(ps(t)-βs)Tvs(t)T(-β)T]T，并得出如下方程：
[0105]
[0106] 其 中 ，
[0107] ε(t)为加速度测量值 含有的白噪声，R为状态矩阵， 为R的共轭矩阵，RT表示矩阵R的转置，I3为标准对角矩阵，M1为智能小车的测量坐标，Y1为智能小车的真实坐标，V1为测量噪声。
[0108] S403、对S402的方程进行离散化，有
[0109]
[0110] 以及在点 处线性化，构造观测方程
[0111] Z＝HZxz+VZ
[0112] 其中k表示离散化的时间，Φ表示状态转移矩阵，G表示输入增益矩阵，Z为观测值，为系统状态，HZ和VZ为系数矩阵。
[0113] 构造卡尔曼预测方程如下
[0114]
[0115] 其滤波更新方程如下
[0116]
[0117] 其中P为卡尔曼估计误差协方差矩阵，K表示卡尔曼增益，Qk＝var(W(k))为建模误差协方差矩阵，C、D为系数矩阵，Rk+1＝D*var(vec(V))*DT为测量误差的协方差矩阵，Θ为修正量。
[0118] S404、由上述卡尔曼滤波方程，计算出智能小车在惯性坐标系的观测坐标ps。
[0119] S5、所述步骤S3得到的智能小车的旋转姿态以及步骤S4得到的智能小车在惯性坐标系的坐标位置和空间位置已知的标志点信息融合计算后输出舵机控制信号以及电机控制信号，分别控制小车的转向和速度。
[0120] 实施例二
[0121] 如图2所示，本实施例公开了一种融合图像传感器和惯性传感器的智能小车控制系统。包括：小车机械结构、接收子系统、传输控制子系统和驱动子系统。所述小车机械结构为小型四轮小车，由舵机带动两个前轮偏转，由电机驱动两个后轮转动。所述接收子系统包括有CMOS摄像头、加速度计和陀螺仪。所述传输控制子系统包括有ADC模数转换器以及单片机。所述驱动子系统包括有直流稳压模块、舵机驱动电路、舵机、电机驱动电路和直流电机。
[0122] 惯性传感器测量出小车的转弯角速度以及线加速度，通过ADC模数转换器传输到单片机，单片机利用获取得到的信息计算出小车的旋转矩阵。如图3所示，CMOS模拟摄像头拍摄得到相交于已知坐标的标志点的直线，单片机对其处理得到处于图像平面并与直线相垂直的法向量。由旋转矩阵和法向量构造状态方程和隐式约束方程即可求出小车的状态矩阵。
[0123] 如图4所示，将关于位置的隐式约束观测方程进行线性化，得到等效的线性观测方程，然后对其设计卡尔曼滤波器，得到其递推方程，求解出小车位置。
[0124] 如图5所示，由小车自身轴线与竖直方向上的夹角，并根据小车坐标和目标点坐标，由几何关系算出小车的转弯半径以及行进弧长，转弯半径以一定的比例转换成PWM信号的占空比由单片机传送到舵机，行进弧长则相对地转换成一定时间和占空比的PWM信号传送到控制电机的H桥电路。
[0125] 上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
