双轮平衡车是一种集环境感知、规划决策、自主驾驶于一体的新型智能运动系统^[1],将其改造成自动摄影机器人可以突显其轻便、灵活、快捷的特点。但由于双轮倒立摆的特点,行驶中时常会影响拍摄图像的质量。为解决此类问题,选择三轴增稳云台^[2]作为摄像头与车体的连接装置,通过控制云台的三轴旋转来消除自身及外来的误差扰动,从而实现摄像头姿态稳定的目的。三轴增稳云台通常使用3个伺服电机进行3个轴维度的角度调节,并且多采用PI(proportion integral,比例-积分)控制策略,但传统的PI控制无法较好地满足车载云台快速响应扰动的控制需求。王日俊等^[3]提出了一种基于改进扰动观测器的模糊自适应跟踪控制算法,在反馈回路中引入补偿控制,用来构建基于速度信号的改进型扰动观测器结构,取得较好的补偿效果,然而该控制策略对模型精度要求较高。王晓晓等^[4]提出采用改进的遗传算法对原有PI控制参数进行寻优,结果表明遗传算法优化后的PID(proportion integral differential,比例-积分-微分)参数系统有效缩短了调节时间,并且无超调产生,证明了遗传算法PID控制的优越性。Li等^[5]提出一种将参数辨识和LQG(linear quadratic gaussian,高斯线性二次方程)扰动抑制算法相结合的自调节算法,能够保持现有控制性能的同时提高LOG算法的鲁棒性,但由于其算法复杂程度较高,参数整定较为繁琐,难以应用于实际工程。陈茂轩等^[6]提出一种结合滑模控制和二型模糊控制的控制策略,利用二型模糊控制能够自适应调节滑模控制器参数的特点,有效抑制了云台姿态调节中的抖动现象。孙方超等^[7]提出了一种自抗扰控制(active disturbance rejection control,ADRC)策略,其核心是将系统输出和输入信号中多个不同量纲转化为统一量纲后直接串联,采用扩张状态观测器实时估计系统内外总扰动量并加以补偿,有效提高了误差反馈的速度。Huang等^[8]在自抗扰控制基础之上提出带宽参数线性整定的思想,改进为线性自抗扰控制(linear active disturbance rejection control,LADRC)策略,解决原ADRC参数整定复杂的缺点,使其更适用于工程领域的应用。

平衡车由于双轮倒立摆的结构特点,在行驶过程中自身姿态会持续进行动态调整,这也给云台姿态稳定带来一定的影响。本文以三轴增稳云台为研究对象,采用四元数空间姿态解算法提取云台位置信号,分析平衡车行驶过程中车载云台姿态扰动的特点,并采用LADRC算法调节云台姿态角,以优化其自稳抗扰动性能。

图1 三轴增稳云台结构
Fig.1 Triaxial stabilization platform structure

车载云台增稳系统由控制器、传感器和三轴机械臂组成。其中,三轴机械臂由3台永磁伺服电机驱动,可在互相垂直的3个维度自由旋转。云台同时配有陀螺仪、加速度计等姿态传感器,用于获取姿态角和速度信息。控制器经过姿态解算算法和滤波算法后将信号传输给各机械臂上的伺服电机,通过电机转动来达到云台自稳定的效果。定义云台三轴分别为偏航轴、俯仰轴和滚动轴^[9],其结构如图1所示。

定义地面坐标系为初始坐标系,云台的本体坐标系为目标坐标系,姿态解算过程即为地面坐标系到云台本体坐标系的角度变换过程,常用的表示方法有欧拉角、方向余弦矩阵和四元数法^[10]。本文采用四元数法作为姿态解算的主要算法,即3次旋转简化为绕某一轴的单次旋转,四元数姿态矩阵如下:

单次旋转角度记作σ,旋转轴记作uⁿ=(u₁,u₂,u₃),式(1)中q_i(i=0,1,2,3)表示如下:

由式(1)可知,四元数构成的姿态矩阵不含三角函数,其计算量显著降低。用四元数表示的偏航角φ、俯仰角θ和滚动角ψ表示如下:

利用四元数法进行姿态解算,可以快速获得云台三轴姿态角度,该空间位置捕获跟踪过程可为系统扰动分析和姿态调节过程传递控制信号。

与常规四轮车辆相比,双轮平衡车具有前后俯仰、水平转向、直立抖动等更多运动维度,同时包含低频和高频扰动,其自稳定控制过程更加复杂。自稳定控制分为纠偏过程和跟踪过程。结合双轮平衡车自身的结构特点,可以将其影响因素分解为前倾后倾扰动、转向离心扰动、直立抖动3个不同控制任务。经过分析可得,扰动可根据车轮的速度差或加速度进行分类,从而选择不同的控制过程。

如图2所示,在双轮平衡车行驶中会出现降速(急刹)或加速的状况,车身会出现前倾或后倾的形态变化,此时车身顶端云台俯仰角平衡产生了扰动。车身顶端云台视角如图3所示,当车身发生倾斜时,云台视角发生改变,需要改变俯仰角度使云台镜头视角继续朝前,俯仰角所需纠偏角度即为车身倾斜角度。

图2 前后倾倒示意
Fig.2 Anteversion and retroversion

图3 车身顶端云台视角示意
Fig.3 Viewing angle of top platform

当车辆质心偏离平衡位置时,受云台自身重力的影响,在俯仰电机轴上会产生扭矩负载。设云台单轴力臂长度为l,电机质量为m,车体加速度为a,此时电机受到的力F和扭矩负载T_l如下:

式(4)中:a(t)为车辆瞬时加速度,可以由平衡车车轮处的速度传感器获取; θ(t)为随时间变化的倾角。设车身高度为h,引用平衡车运动方程^[11]可得两者的关系,表示如下:

h(d²θ(t))/(dt²)=gsin[θ(t)]-a(t)cos[θ(t)]+hx(t)。(5)

式(5)中:x(t)为外界干扰加速度。若忽略外界干扰加速度,且当θ很小时,车身运动学方程可简化为

h(d²θ(t))/(dt²)=gθ(t)-a(t)。(6)

带入电机扭矩负载

T_l(t)=l[mgθ(t)-ma(t)]。(7)

经分析可见,前后倾斜情况下保持平衡就是要保持云台俯仰角(镜头视角)相对地面坐标系不变,一直处于水平状态,该调节过程即为一种纠偏过程。

图4 转向俯视图
Fig.4 Steering overlook

由于双轮平衡车体积小、车身高,在平衡车转向时,处于顶端的云台系统产生偏航角的偏差,同时施加给云台电机离心力产生扭矩负载。转向俯视图如图4所示,为维持摄像头的既定朝向,电机偏角即为车身转过角度φ。

根据双轮平衡车的转向原理可知,它依靠两轮的转速差来完成转向,设此时左轮、右轮转速分别为v₁、v₂,两轮间距为D,则此时车身前进速度v₀和转向速度ω表示如下:

因此当两车轮产生速度差值时,车身转角即偏航角误差φ(t)表示如下:

设平衡车整体质量(包括云台)为M,转向所产生的扭矩负载

分析可知,水平转向与前后倾斜扰动情形不同,偏航角不再相对于地面坐标系稳定不变,而是相对于机体坐标系维持不变,从而使得摄像头视轴朝向始终面向正前方,该过程描述为跟踪过程。

双轮平衡车的直立抖动与汽车大致相同,都是由行驶道路的凹凸和坑洼引起的车身上下颠簸,这影响云台的滚动角平衡。相比于前两类扰动情形,直立扰动则较为简单,不需要考虑前行速度、姿态及惯性,只需利用云台上的陀螺仪传感器的z轴信号,便可以检测出车身垂直抖动的幅度,即滚动角的误差值,该调节过程是一种纠偏过程。

除了上述3种双轮平衡车特有的扰动以外,在行驶中也会出现例如刮风、碰撞、特殊路段等外界扰动因素,经过云台内部的位置传感器获取数据后,反馈给位置控制器再由电机进行角度纠正。但云台电机自主调节也存在极限,一旦误差角度超出可调节范围,或转动扭矩超出力臂承受范围,就会使云台无法进行自主调节。例如转向过快、颠簸幅度过大、外界施加干扰过大等都会导致上述调节效果不佳甚至无法调节的情况。为了避免上述突发或严重工况,本文试验都在较为稳定的行驶情况下进行。

控制系统由外部扰动观测系统和自稳定控制系统构成。外部扰动观测系统对外部扰动进行实时跟踪与分析,车轮的速度传感器对两车轮的速度差和加速度进行观察,利用扰动分析器获取扰动特征角度,再将角度信号传输给自稳定控制系统。自稳定控制系统用于实现轴臂机构的稳定调节,由控制电机转动的电流闭环、速度闭环及位置闭环构成。利用各轴之间的永磁同步电机进行偏差角度的补偿,使云台达到稳定状态。同时,使用云台位置传感器对内部和外部的位置扰动进行实时跟踪,形成实时反馈的闭环控制系统。控制系统流程如图5所示。

图5 控制系统流程图
Fig.5 Flow chart of control system

图6 二阶LADRC系统结构
Fig.6 System structure of second order LADRC

考虑云台的姿态干扰信号在三维轴向上线性独立,采用LADRC策略以减少系统参数。该控制策略有三大模块:线性跟踪微分器^[12](tracking differentiator,TD)、状态扩张观测器^[13](linear extended state observer,LESO)、状态误差反馈控制律(linear state error feedback,LSEF)。以俯仰轴θ为例,二阶LADRC系统结构如图6所示,将期望角θ作为系统输入值,经过跟踪微分器变为一个微分量,K_P,K_D状态误差反馈控制律参数; [z₁,z₂,z₃]^T为状态观测器的状态矩阵; b₀为系统估计值; u和y则作为云台系统的输入和输出值。

将LSEF的参数线性处理,此时n阶误差反馈控制律则改写如下:

u=k₁e₁+…+k_ne_n-(z_n+1)/(b₀)。(11)

设控制律带宽为ω_c,则式(11)可改写为n阶多项式如下:

λ_c=sⁿ⁺¹+k_nsⁿ+…+k₁=(s+ω_c)ⁿ。(12)

此时的二阶系统中控制律参数K_P=ω²_c,K_D=2ω_c,所以ω_c是影响系统响应速度和稳态精度的关键因素。再将LESO的参数进行线性处理^[14],系统参数整定为与观测器带宽ω₀相关的n阶多项式:

λ₀=sⁿ⁺¹+β₁sⁿ+…+β_ns¹+β_n+1s⁰=(s+ω₀)ⁿ⁺¹。(13)

β=[β₁,β₂,…,β_n]^T为状态扩张观测器的增益矩阵,二阶系统参数β₁=6ω₀,β₂=3ω²₀,β₃=ω³₀,其中ω₀越大代表着扩张观测器精度越高,但也会出现更大的观测噪声,因此需根据传感器信噪比选取合理的ω₀值。传统PI控制要等到误差发生后进行调节,而LADRC系统由于跟踪微分器的存在,能够准确地跟踪到目标的微分,起到一定的预估作用。

考虑到控制系统中内部电流环需要简单的控制结构以降低控制器的运算负荷,外部位置和速度环需要更高的精度和更快的响应速度以改善自稳定效果,所以试验中位置闭环和速度闭环采用LADRC系统,并与传统PI控制进行比较。处于内环的电流闭环依旧使用PI控制方法。LADRC参数整定:b₀=0.01,ω₀=30,ω_c=15; PI参数整定:K_p=4,K_i=10。

为了达到良好的对比效果,分别用这两种方法对预设的云台姿态进行调节。初始姿态角为0°,期望姿态角为17°、12°、7°,并在2 s时施加一个扭矩为0.5 N·m的干扰信号。PI与LADRC对比调试结果如图7所示,可以明显看出LADRC系统在角度跟踪时只需0.4 s,而PI控制则需0.5 s以上,且出现明显超调,幅度达到1°。当遇到干扰时,LADRC在0.5 s左右达到自主纠偏的效果并保持稳定,PI控制则需要0.8 s左右的调节时间,且存在波动,控制性能不及LADRC。

根据上述对平衡车扰动的分析,模拟平衡车行驶中对云台施加扰动负载信号,采用LADRC策略调节云台姿态:1～2 s,平衡车以a(t)=t的加速度启动,由式(6)～(7)计算出其干扰角度幅值为4°,扭矩幅值为0.8 N·m; 2～4 s,经过坑洼不平的道路,图8是路面坑洼工况引发的扭矩扰动特性图^[15],反映滚动轴负载扭矩的变化,峰-峰值达到1.96 N·m; 4～6 s,平衡车以两轮间0.2 m/s的速度差匀速转向,由式(9)～(10)计算出其转向角度为5°,扭矩幅值为0.5 N·m; 6～7 s,平衡车减速直至车身停止。

图7 PI与LADRC对比
Fig.7 Comparison between PI and LADRC

图8 路面坑洼工况引发的云台扭矩扰动特性
Fig.8 Disturbance characteristics of platform torque caused by road potholes

三轴的角度调节效果反映了云台姿态的调节效果,三轴电流信号反映了调节的稳定性和波动大小,如图9～10所示。由图9可知,三轴期望角分别为22°、34°、47°,启动和急停产生俯仰角的误差在5°以内,电机调节时间为0.2 s左右。在转向和路面坑洼产生偏航角和滚动角误差时,误差会达到3°～5°,电机调节时间会增加0.5～1 s。转向后偏航角也达到补偿的效果,变为转向后的50°。由图 10可知,干扰频率较小时,例如4～6 s转向过程,电机能够迅速恢复至期望角,且无振荡现象发生。干扰频率过大时,例如2～4 s坑洼路段,电机则会出现一定的震荡和超调现象,但波动并不大,这符合控制需求。

图9 云台位置响应
Fig.9 Response diagram of platform position

图 10 云台电流响应
Fig.10 Response diagram of platform current

基于LADRC理论分析及仿真试验,试验采用STM32F407芯片作为核心运算处理器,DRV8301作为电机驱动芯片来驱动单轴永磁同步电机(图 11),使电机达到预期位置角,且在干扰出现时进行自稳定调节。试验电机位置响应结果如图 12所示,预期位置角设定为20°、25°、30°,PI控制需要1 s达到预期角度,且存在0.5°左右超调,而LADRC仅需0.8 s达到预期角,且无超调,调节速度提高20%。在姿态稳定后晃动云台,由PI控制的云台三轴扰动角度分别为+6.5°、-6.6°、-10°,云台姿态在0.8 s后再次达到稳定状态,而在LADRC下的云台扰动角度为+5.5°、-6°、-9.5°,扰动角度减少0.5°～1°,控制精度提高10%～15%,调节速度也提高0.3 s。试验证明LADRC策略可以使电机位置角快速调节,且有很高的稳定性能。这与理论分析和仿真试验结果一致,验证了本研究方法的有效性。

与仿真相比,试验中电机电流较小,且需要预先捕捉转子位置,所以调节时间会慢0.2 s左右。又由于电机内部电磁谐波干扰,三轴位置角在达到稳定状态后还会存在波动,波动稳定在±0.2°左右。

图 11 试验平台构建
Fig.11 Establishment of experimental platform

图 12 试验电机位置响应
Fig.12 Response diagram of experimental motor

本文对双轮平衡车摄影机器人的三轴增稳云台进行研究,将其与车身多个传感器相互联动,使云台在双轮平衡车行驶时进行更快速更稳定的姿态角调节。试验以三轴云台作为控制对象,将双轮平衡车行驶中特有的扰动作为扰动输入,用四元数法和LADRC作为控制策略,研究结果表明,在双轮平衡车所独有的扰动特性下,LADRC策略相比PI控制调节速度提高20%,且基本上无超调,控制精度提高10%～15%,表现出更强的自稳定控制能力和鲁棒性,证实了LADRC策略更适应双轮平衡车扰动特性下的云台姿态调节,能提高云台自稳定效率。但在环境更复杂,如平衡车转速过快、云台力臂负载过大时,仍有反应慢、调节效果不佳的情况,这有待进一步改进。