一种车载摄像头云台伺服系统及控制方法与流程

本发明涉及车载摄像头，特别是涉及一种车载摄像头云台伺服系统及控制方法。

背景技术：

无人驾驶车辆的安全性一直是该领域的热点问题，摄像头作为视觉传感器为无人驾驶车辆提供了大量数据信息。然而由于路面不平、转向、制动、加速等因素，实际行驶中车辆存在各种不确定的姿态变化，构成了摄像头运行的扰动因素。为保证视频数据的稳定性和连续性，车载摄像头三轴云台应运而生，它是一种三轴的可旋转机械结构。与无人机航拍、手持拍摄等领域的摄像头不同，无人驾驶摄像头是实现无人驾驶功能的关键设备，因此提升摄像头三轴云台的稳定性，对无人驾驶车辆的安全具有重要意义。。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种车载摄像头云台伺服系统及控制方法，提升了摄像头三轴云台伺服系统实时控制的动态性能和抗扰性能，为无人驾驶车辆的安全性提供保障。

本发明的目的是通过以下技术方案来实现的：一种车载摄像头云台伺服系统，包括摄像头三轴云台和伺服控制装置；

所述摄像头三轴云台包括俯仰电机、侧倾电机和横摆电机、侧倾臂、俯仰臂、横摆臂、云台顶座、摄像头、俯仰轴轴承和配重块；所述俯仰电机包括俯仰电机定子和俯仰电机转子；所述横摆电机包括横摆电机定子和横摆电机转子；所述侧倾电机包括侧倾电机定子和侧倾电机转子；

所述摄像头经俯仰臂与俯仰电机转子固连，所述俯仰电机转子用于实现俯仰方向的运动；所述配重块螺纹连接在俯仰臂上，用于补偿侧倾方向上由于俯仰电机自重所造成的重力不平衡力矩；所述俯仰臂由俯仰轴轴承进行约束；所述俯仰电机定子经侧倾臂与侧倾电机转子固连，所述侧倾电机转子用于实现侧倾方向的运动；侧倾电机定子经横摆臂与横摆电机转子固连；所述横摆电机转子用于实现横摆方向的运动；所述横摆电机定子与云台顶座固连，所述云台顶座靠近车内顶部的内后视镜进行固定，从而将整个摄像头三轴云台固连到车内顶部的内后视镜附近；所述俯仰电机、侧倾电机和横摆电机内部均设置有位置传感器；

所述伺服控制装置包括：

惯性测量单元，安装于摄像头上，用于测量摄像头的角速度和角位移信息，并利用四元数互补滤波算法生成精确角位移信息；

执行机构控制单元，用于依据俯仰电机、横摆电机、侧倾电机内部的位置传感器，生成三个方向上驱动电桥igbt开关顺序信息；基于三个方向上的目标电压信息，生成驱动电桥igbt占空比，通过驱动电桥实现俯仰、侧倾、横摆三个方向电机转速的开环控制；

角速度环控制单元，基于执行机构控制单元，构建角速度环控制器模型，所述角速度环控制模型经粒子群算法参数优化后，根据惯性测量单元得到的实际角速度信息，与目标角速度信息结合，生成电机驱动电压pwm信号的占空比信息；

角位移环控制单元，用于在优化后的角速度环控制器模型基础上，构建角位移环控制器模型；所述角位移环控制器模型经粒子群算法参数优化后，根据用户设定的目标转角信息与惯性测量单元得到的实际角位移信息，生成目标角速度发送给角速度环控制器模型，实现双闭环控制。

优选地，所述俯仰臂上设置有螺纹，配重块上设置有螺纹孔，所述螺纹与螺纹孔配合，实现俯仰臂与配重块的连接，并使得配重块在俯仰臂上位置可调。所述惯性测量单元包括：加速度计，用于测量摄像头的角位移值；陀螺仪，用于测量摄像头的瞬时角速度值；互补计算单元，用于将陀螺仪测量的角速度值与加速度计测量的角度值进行四元数互补计算，得到精确的瞬时角度值。所述俯仰电机、侧倾电机和横摆电机均为无刷直流电机。

一种车载摄像头云台伺服系统的控制方法，包括以下步骤：

s1.利用安装于摄像头上的惯性测量单元，测量摄像头的角速度和角位移信息，并利用四元数互补滤波算法生成精确角位移信息；

s2.根据摄像头三轴云台的机械结构，结合俯仰电机、侧倾电机和横摆电机参数；分别在俯仰、侧倾、横摆三个方向进行simulink建模，依据电机内部的位置传感器，生成驱动电桥igbt开关顺序信息；基于目标电压信息，生成驱动电桥igbt占空比，通过驱动电桥实现俯仰、侧倾、横摆三个方向电机转速的开环控制，得到执行机构控制模型，即俯仰电机、侧倾电机和横摆电机的simulink控制模型；

s3.基于执行机构控制模型，在simulink中构建角速度环控制器模型，采用粒子群优化算法迭代优化角速度环控制器模型的参数；参数优化后的角速度环控制器根据惯性测量单元得到的实际角速度信息，结合目标角速度信息，生成电机驱动电压pwm信号的占空比信息；

s4.基于角速度环控制器模型，在simulink中构建角位移环控制器模型，利用粒子群优化方法对角位移环控制器模型进行参数优化；参数优化后的角位移环控制器模型根据用户设定的目标转角信息与惯性测量单元得到的实际角位移信息，生成目标角速度发送给角速度环控制器模型，实现双闭环控制。

进一步地，所述步骤s1中，四元数更新表达式：

被测物体坐标系b向地面坐标系r转化的四元数矩阵为：

将式(2)记为如下形式：

姿态角的表达式为：

其中，θ为俯仰角，γ为侧倾角，ψ为横摆角；

利用低通滤波器处理加速度计得到的角度值，利用高通滤波器处理陀螺仪解算出的角度，然后加权求和得到最终输出角度；

其中，β1为加速度计得到的角度值，β2为陀螺仪解算得到的角度值，τ为时间常数。

进一步地，所述步骤s2包括以下子步骤：

s201.在solidworks中建立摄像头三轴云台的三维模型，配置三维模型材料，求出三根旋转轴上负载的转动惯量；

s202.在simulink中搭建执行机构控制模型，配置包含相间电感、线电阻、极对数、转轴粘性系数的电机参数和负载转动惯量；

s203.在simulink中搭建编码器模块，基于位置传感器信息计算igbt电桥开关顺序；

s204.在simulink中搭建pwm模块和恒压电源模块；

s205.整个模型的输入为驱动电压信号，即角速度环控制单元输出的控制量u，其满足:

即得到构建的执行机构控制模型。

进一步地，所述步骤s3包括：

s301.将simulink保存为.mdl的模型格式，在模型中设定阶跃目标角速度，将角速度环控制器参数kp、ki作为模型输入量，速度环阶跃响应的实际转角作为模型输出量；

s302.在matlab中建立pso主函数，选取粒子维数为2，粒子数目为50，最大迭代次数为100，在粒子速度更新公式(7)中，i表示第i个粒子，k表示迭代次数，d表示维度，x表示粒子位置，pbest和gbest分别表示个体历史最优位置和全局最优位置，c1、c2分别为个体学习率和群体学习率，r1、r2为[0,1]，w为惯性权重，w从0.9至0.4线性递减，提高搜索效率并降低陷入局部最优的几率；

vid^k＝w×vid^k-1+c1×r1×(pbestid-xid^k-1)+c2×r2×(gbestd-xid^k-1)(7)

xid^k＝xid^k-1+vid^k-1(8)

s303.适应度函数为超调量与误差绝对值的加权和，超调量项的惯性权重为0.009，误差绝对值项的惯性权重为1，如下所示：

yfit＝w1×δ+∫w2×|e|×dt(9)

s304.每次迭代中基于sim()命令调用simulink模型，计算适应度函数的值，对粒子速度进行更新，进而对粒子位置进行更新(8)；粒子位置和速度应当满足约束条件(10)、(11)；

最终达到最大迭代次数，得到kp、ki的最优解。

进一步地，所述步骤s4包括：

s401.构建扩张状态观测器eso，二阶eso离散系统的基本过程如下：

e1＝z1-y(12)

z1(k+1)＝z1(k)+ts×(z2-β01×e1)(13)

z2(k+1)＝z2(k)+ts×(z3-β02×fal(e1,0.5,δ)+b×u)(14)

z3(k+1)＝z3(k)-ts×β03×fal(e1,0.25,δ)(15)

其中z为观测得到的状态向量，u是控制量，y为实际输出量，ts为步长，β01、β02、β03、α和δ通过经验确定，b为待优化的参数；

s402.构建跟踪微分器td，基于fhan建立的离散系统td如下：

fh＝fhan(x1(k)-v(k),x2(k),r0,h0)(17)

x1(k+1)＝x1(k)+ts×x2(k)(18)

x2(k+1)＝x2(k)+ts×fh(19)

其中v为控制目标，x为跟踪后的控制目标，ts为步长；fhan的表达式如下所示：

d＝r0×h0²(20)

a0＝h0×x2(21)

y＝x1+a0(22)

a＝(a0+y-a2)×sy+a2(26)

其中x为输入的状态向量，r0、h0为通过经验确定的系数；

s403.构建非线性状态误差反馈nlsef，非线性状态误差反馈控制率的离散过程如下：

u0＝kp×fal(e1,a1,δ)+kd×fal(e2,a2,δ)(29)

其中u为补偿扰动后输出的控制量，e1、e2为观测的状态向量误差，a1、a2、δ和b0为通过经验确定的系数；kp、kd分别为比例系数和微分系数，需要进行优化；

s404.将simulink模型保存为.mdl格式，输入为kp、kd、b三个优化参数，输出为阶跃响应的实际输出角位移；

s405.在matlab中建立pso主函数，选取粒子维数为2，粒子数目为50，最大迭代次数为100，粒子速度更新公式的惯性权重从0.9至0.4线性递减，提高搜索效率并降低陷入局部最优的几率；

s406.适应度函数为超调量与误差绝对值的加权和，超调量项的惯性权重为0.009，误差绝对值项的惯性权重为1，如下所示：

yfit＝w1×δ+∫w2×|e|×dt(31)

s407.每次迭代中基于sim()命令调用simulink模型，计算适应度函数的值，对粒子速度进行更新，进而对粒子位置进行更新；最终达到最大迭代次数，得到kp、kd、b的最优解。

本发明的有益效果是：本发明构建了以角位移环为外环、角速度环为内环的双闭环控制框架。基于非线性误差反馈控制率(nlsef)，对控制量的各项进行非线性组合，提高了伺服系统的动态性能；基于扩张状态观测器(eso)实时观测系统的扰动并对其进行补偿，提高了伺服系统的抗干扰能力。依据经验与优化结合的方法角位移环控制器的参数进行整定，以超调量δ和误差绝对值|e|的加权和作为粒子群优化的适应度函数，该方法降低了角位移环控制器参数整定的复杂度，又保证伺服系统达到相应的控制效果。

附图说明

图1为摄像头三轴云台的结构示意图；

图2为伺服控制装置的原理示意图；

图3为本发明的方法流程图；

图中，1-侧倾臂，2-俯仰电机定子，3-俯仰电机转子，4-俯仰臂，5-横摆臂，6-横摆电机定子，7-云台顶座，8-横摆电机转子，9-侧倾电机定子，10-侧倾电机转子，11-摄像头，12-俯仰轴轴承，13-配重块。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

如图1～2所示，一种车载摄像头云台伺服系统，包括摄像头三轴云台和伺服控制装置；

所述摄像头三轴云台包括俯仰电机、侧倾电机和横摆电机、侧倾臂1、俯仰臂4、横摆臂5、云台顶座7、摄像头11、俯仰轴轴承12和配重块13；所述俯仰电机包括俯仰电机定子2和俯仰电机转子3；所述横摆电机包括横摆电机定子6和横摆电机转子8；所述侧倾电机包括侧倾电机定子9和侧倾电机转子10；

所述摄像头11经俯仰臂4与俯仰电机转子3固连，所述俯仰电机转子3用于实现俯仰方向的运动；所述配重块13螺纹连接在俯仰臂4上，用于补偿侧倾方向上由于俯仰电机自重所造成的重力不平衡力矩；所述俯仰臂4由俯仰轴轴承12进行约束；所述俯仰电机定子2经侧倾臂1与侧倾电机转子10固连，所述侧倾电机转子10用于实现侧倾方向的运动；侧倾电机定子9经横摆臂5与横摆电机转子8固连；所述横摆电机转子8用于实现横摆方向的运动；所述横摆电机定子6与云台顶座7固连，所述云台顶座7靠近车内顶部的内后视镜进行固定，从而将整个摄像头三轴云台固连到车内顶部的内后视镜附近；所述俯仰电机、侧倾电机和横摆电机内部均设置有位置传感器；

所述伺服控制装置包括：

惯性测量单元，安装于摄像头11上，用于测量摄像头11的角速度和角位移信息，并利用四元数互补滤波算法生成精确角位移信息；

执行机构控制单元，用于依据俯仰电机、横摆电机、侧倾电机内部的位置传感器，生成三个方向上驱动电桥igbt开关顺序信息；基于三个方向上的目标电压信息，生成驱动电桥igbt占空比，通过驱动电桥实现俯仰、侧倾、横摆三个方向电机转速的开环控制；

角速度环控制单元，基于执行机构控制单元，构建角速度环控制器模型，所述角速度环控制模型经粒子群算法参数优化后，根据惯性测量单元得到的实际角速度信息，与目标角速度信息结合，生成电机驱动电压pwm信号的占空比信息传输给执行机构控制单元的驱动电桥；

角位移环控制单元，用于在优化后的角速度环控制器模型基础上，构建角位移环控制器模型；所述角位移环控制器模型经粒子群算法参数优化后，根据用户设定的目标转角信息与惯性测量单元得到的实际角位移信息，生成目标角速度发送给角速度环控制器模型，实现双闭环控制。

在本申请的实施例中，执行机构控制单元主要包括编码器和驱动电桥，其中，编码器用于依据俯仰电机、横摆电机、侧倾电机内部的位置传感器，生成三个方向上驱动电桥igbt开关顺序信息，基于三个方向上的目标电压信息，生成驱动电桥igbt占空比；驱动电桥用于实现俯仰、侧倾、横摆三个方向电机转速的开环控制。

在本申请的实施例中，所述俯仰臂4上设置有螺纹，配重块13上设置有螺纹孔，所述螺纹与螺纹孔配合，实现俯仰臂4与配重块13的连接，并使得配重块13在俯仰臂4上位置可调。所述惯性测量单元包括：加速度计，用于测量摄像头的角位移值；陀螺仪，用于测量摄像头的瞬时角速度值；互补计算单元，用于将陀螺仪测量的角速度值与加速度计测量的角度值进行四元数互补计算，得到精确的瞬时角度值。所述俯仰电机、侧倾电机和横摆电机均为无刷直流电机俯仰、侧倾、横摆三个方向的定义为：俯仰轴位于水平面内，方向垂直于车辆前进方向；侧倾轴位于水平面内，方向指向车辆前进方向；横摆轴垂直于大地坐标系水平面向上。各个结构的连接方式为：俯仰电机定子2、侧倾电机定子9、横摆电机定子6与相应固连结构的固连方式为4根螺钉。俯仰电机转子3、侧倾电机转子10、横摆电机转子7与相应固连结构的固连方式为4根螺钉。云台顶座7与车内顶部的固连方式为4根螺钉。电机线束的导出方式为：俯仰电机线束经侧倾臂1上的孔导出，侧倾电机线束经横摆臂上5的孔导出，横摆电机线束经云台顶座7上的孔导出。

如图3所述，所述的一种车载摄像头云台伺服系统的控制方法，包括以下步骤：

s1.利用安装于摄像头11上的惯性测量单元，测量摄像头11的角速度和角位移信息，并利用四元数互补滤波算法生成精确角位移信息；

s2.根据摄像头三轴云台的机械结构，结合俯仰电机、侧倾电机和横摆电机参数；分别在俯仰、侧倾、横摆三个方向进行simulink建模，依据电机内部的位置传感器，生成驱动电桥igbt开关顺序信息；基于目标电压信息，生成驱动电桥igbt占空比，通过驱动电桥实现俯仰、侧倾、横摆三个方向电机转速的开环控制，得到执行机构控制模型，即俯仰电机、侧倾电机和横摆电机的simulink控制模型；

s3.基于执行机构控制模型，在simulink中构建角速度环控制器模型，采用粒子群优化算法迭代优化角速度环控制器模型的参数；参数优化后的角速度环控制器根据惯性测量单元得到的实际角速度信息，结合目标角速度信息，生成电机驱动电压pwm信号的占空比信息；

s4.基于角速度环控制器模型，在simulink中构建角位移环控制器模型，利用粒子群优化方法对角位移环控制器模型进行参数优化；参数优化后的角位移环控制器模型根据用户设定的目标转角信息与惯性测量单元得到的实际角位移信息，生成目标角速度发送给角速度环控制器模型，实现双闭环控制。

所述步骤s1中，四元数更新表达式：

被测物体坐标系b向地面坐标系r转化的四元数矩阵为：

将式(2)记为如下形式：

姿态角的表达式为：

其中，θ为俯仰角，γ为侧倾角，ψ为横摆角；

利用低通滤波器处理加速度计得到的角度值，利用高通滤波器处理陀螺仪解算出的角度，然后加权求和得到最终输出角度；

其中，β1为加速度计得到的角度值，β2为陀螺仪解算得到的角度值，τ为时间常数。

所述步骤s2包括以下子步骤：

s201.在solidworks中建立摄像头三轴云台的三维模型，配置三维模型材料，求出三根旋转轴上负载的转动惯量；

s202.在simulink中搭建bldcm模型(执行机构控制模型)，配置包含相间电感、线电阻、极对数、转轴粘性系数的电机参数和负载转动惯量，基于位置传感器构建igbt电桥开关顺序；

s203.在simulink中搭建pwm模块和恒压电源模块；

s204.整个模型的输入为驱动电压信号，即角速度环控制单元输出的控制量u，其满足:

即得到构建的三维控制模型。

所述步骤s3包括：

s301.将simulink保存为.mdl的模型格式，在模型中设定阶跃目标角速度，将pi控制器(角速度环控制器)参数kp、ki作为模型输入量，速度环阶跃响应的实际转角作为模型输出量；

s302.在matlab中建立pso主函数，选取粒子维数为2，粒子数目为50，最大迭代次数为100，在粒子速度更新公式(7)中，i表示第i个粒子，k表示迭代次数，d表示维度，x表示粒子位置，pbest和gbest分别表示个体历史最优位置和全局最优位置，c1、c2分别为个体学习率和群体学习率，r1、r2为[0,1]，w为惯性权重，w从0.9至0.4线性递减，提高搜索效率并降低陷入局部最优的几率；

vid^k＝w×vid^k-1+c1×r1×(pbestid-xid^k-1)+c2×r2×(gbestd-xid^k-1)(7)

xid^k＝xid^k-1+vid^k-1(8)

s303.适应度函数(9)为超调量与误差绝对值的加权和，超调量项的惯性权重为0.009，误差绝对值项的惯性权重为1；

yfit＝w1×δ+∫w2×|e|×dt(9)

s304.每次迭代中基于sim()命令调用simulink模型，计算适应度函数的值，对粒子速度进行更新，进而对粒子位置进行更新(8)；粒子位置和速度应当满足约束条件(10)、(11)；

最终达到最大迭代次数，得到kp、ki的最优解。

所述步骤s4包括：

s401.构建扩张状态观测器eso，二阶eso离散系统的基本过程如下：

e1＝z1-y(12)

z1(k+1)＝z1(k)+ts×(z2-β01×e1)(13)

z2(k+1)＝z2(k)+ts×(z3-β02×fal(e1,0.5,δ)+b×u)(14)

z3(k+1)＝z3(k)-ts×β03×fal(e1,0.25,δ)(15)

其中z为观测得到的状态向量，u是控制量，y为实际输出量，ts为步长，β01、β02、β03、α和δ通过经验确定，b为待优化的参数；

s402.构建跟踪微分器td，基于fhan建立的离散系统td如下：

fh＝fhan(x1(k)-v(k),x2(k),r0,h0)(17)

x1(k+1)＝x1(k)+ts×x2(k)(18)

x2(k+1)＝x2(k)+ts×fh(19)

其中v为控制目标，x为跟踪后的控制目标，ts为步长；fhan的表达式如下所示：

d＝r0×h0²(20)

a0＝h0×x2(21)

y＝x1+a0(22)

a＝(a0+y-a2)×sy+a2(26)

其中x为输入的状态向量，r0、h0为通过经验确定的系数；

s403.构建非线性状态误差反馈nlsef，非线性状态误差反馈控制率(nlsef)的离散过程如下：

u0＝kp×fal(e1,a1,δ)+kd×fal(e2,a2,δ)(29)

其中u为补偿扰动后输出的控制量，e1、e2为观测的状态向量误差，a1、a2、δ和b0为通过经验确定的系数；kp、kd分别为比例系数和微分系数，需要进行优化；

s404.将simulink模型保存为.mdl格式，输入为kp、kd、b三个优化参数，输出为阶跃响应的实际输出角位移；

s405.在matlab中建立pso主函数，选取粒子维数为2，粒子数目为50，最大迭代次数为100，粒子速度更新公式的惯性权重从0.9至0.4线性递减，提高搜索效率并降低陷入局部最优的几率；

s406.适应度函数(31)为超调量与误差绝对值的加权和，超调量项的惯性权重为0.009，误差绝对值项的惯性权重为1；

yfit＝w1×δ+∫w2×|e|×dt(31)

s407.每次迭代中基于sim()命令调用simulink模型，计算适应度函数的值，对粒子速度进行更新，进而对粒子位置进行更新；最终达到最大迭代次数，得到kp、kd、b的最优解。

本发明构建了以角位移环为外环、角速度环为内环的双闭环控制框架。基于非线性误差反馈控制率(nlsef)，对控制量的各项进行非线性组合，提高了伺服系统的动态性能；基于扩张状态观测器(eso)实时观测系统的扰动并对其进行补偿，提高了伺服系统的抗干扰能力。依据经验与优化结合的方法角位移环控制器的参数进行整定，以超调量δ和误差绝对值|e|的加权和作为粒子群优化的适应度函数，该方法降低了角位移环控制器参数整定的复杂度，又保证伺服系统达到相应的控制效果。

以上所述是本发明的优选实施方式，应当理解本发明并非局限于本文所披露的形式，不应该看作是对其他实施例的排除，而可用于其他组合、修改和环境，并能够在本文所述构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。