[发明专利]一种两轮自平衡车集中式编队控制方法

权利要求书

1.一种两轮自平衡车集中式编队控制方法，其特征在于，所述两轮自平衡车集中式编队控制方法，包括：

步骤S1、根据领航平衡车的位置信息和目标队形结构，计算跟随平衡车的理想位置信息，所述位置信息包括坐标和方位角，包括：

步骤S1.1、定义(x y θ)^T为一阶非完整性轮式平衡车在世界坐标系下的横坐标、纵坐标和方位角，则平衡车的运动学方程为：

其中，v为平衡车当前的实际线速度，ω为平衡车当前的实际角速度；

步骤S1.2、获取领航平衡车R_i的位置信息为P_i＝[x_i y_i θ_i]^T，跟随平衡车R_j当前的实际位置信息为P_j＝[x_j y_j θ_j]^T，则根据控制法，计算跟随平衡车R_j的理想位置信息P_j^d为：

其中，跟随平衡车R_j当前的实际位置信息为P_j满足以下关系式：

其中，L_ij^d和分别表示跟随平衡车R_j和领航平衡车R_i的理想距离和理想相对转角，L_ij和分别表示跟随平衡车R_j和领航平衡车R_i的实际距离和实际相对转角；

步骤S2、由跟随平衡车当前的实际位置信息和所述理想位置信息，计算跟随平衡车的位置误差，所述位置误差包括坐标误差和方位角误差，包括：

步骤S2.1、根据跟随平衡车当前的实际位置信息和理想位置信息，得到跟随平衡车R_j在世界坐标系下的位置误差为：

其中，为世界坐标系下坐标x方向的误差，为世界坐标系下坐标y方向的误差，为世界坐标系下方位角误差；

步骤S2.2、将世界坐标系下的位置误差转换至平衡车坐标系下，得到平衡车坐标系下跟随平衡车的位置误差为：

其中，e_j1表示平衡车坐标系下的坐标x方向的误差，e_j2表示平衡车坐标系下的坐标y方向的误差，e_j3表示平衡车坐标系下的方位角误差；

步骤S2.3、对平衡车坐标系下跟随平衡车的位置误差进行求导，得到：

其中，为e_j1的导数，为e_j2的导数，为e_j3的导数，v_j为跟随平衡车R_j当前的实际线速度，v_i为领航平衡车R_i当前的实际线速度，ω_j为跟随平衡车R_j当前的实际角速度，ω_i为领航平衡车R_i当前的实际角速度，d_j表示跟随平衡车R_j的重心到轮轴中心的距离；

步骤S3、构建运动学控制器，采用运动学控制器根据位置误差输出控制跟随平衡车的理想线速度和理想角速度，包括：

构建的运动学控制器如下：

其中，k_x、k_y、k_θ分别表示x、y、θ的正向量常数；

运动学控制器输出的控制跟随平衡车的理想线速度为v_jc、理想角速度为ω_jc；

步骤S4、判断采样时间内按照理想角速度旋转的角度与跟随平衡车实际旋转的角度是否相同，若相同则向跟随平衡车输出理想角速度；否则构建增量式PD控制器，根据采样时间内旋转的角度误差输出实际角速度至跟随平衡车，包括：

步骤S4.1、设置采样时间为ΔT；

步骤S4.2、则在采样时间内跟随平衡车按照理想角速度ω_jc旋转的角度为：θ₁＝ω_jc*ΔT；

步骤S4.3、获取在采样时间内跟随平衡车实际旋转的角度为θ₂，则采样时间内旋转的角度误差为e_θ＝θ₁-θ₂；

步骤S4.4、若e_θ＝0，则向跟随平衡车输出理想角速度ω_jc，并结束本次的角度调控；否则执行下一步；

步骤S4.5、构建增量式PD控制器如下：

Δθ＝λe_θ(k)+βe_θ(k-2)

其中，Δθ为增量式PD控制器输出的实际角速度，e_θ(k)为k时刻的角度误差，e_θ(k-2)为k-2时刻的角度误差，k_p为比例系数，T_d为微分系数，T为采样周期；并将增量式PD控制器输出的实际角速度Δθ发送至跟随平衡车；

步骤S5、判断采样时间内按照理想线速度计算的坐标与跟随平衡车实际运行的坐标是否相同，若相同则向跟随平衡车输出理想线速度；否则构建预测控制器，根据采样时间内运行的坐标误差输出实际线速度至跟随平衡车，包括：

步骤S5.1、设置采样时间为ΔT；

步骤S5.2、则在采样时间内跟随平衡车按照理想线速度v_jc计算得到的对应时刻的坐标为(x_d,y_d)；

步骤S5.3、获取在采样时间内跟随平衡车运行的对应时刻的实际坐标为(x_r,y_r)，则采样时间内运行的坐标误差如下：

其中，x_e,y_e分别为x、y方向的误差；

步骤S5.4、若x_e＝0且y_e＝0，则取所述理想线速度v_jc作为待发送线速度，并执行步骤S5.7；否则执行步骤S5.5；

步骤S5.5、计算采样时间内运行的坐标误差的微分值为：

其中，v为跟随平衡车当前的实际线速度，θ为跟随平衡车当前的实际方位角；

设置中间虚拟控制规律，得到虚拟的x、y方向的线速度如下：

其中，u₁为虚拟的y方向的线速度，u₂为虚拟的y方向的线速度，k₁为常数，k₂为常数；

步骤S5.6、仅考虑x方向的位置误差，即设置y_e＝0，构建预测控制器为：

将预测控制器输出的实际线速度v_r作为待发送线速度；

步骤S5.7、取所述待发送线速度作为当前k时刻的线速度v(k)，根据以下公式判断线速度v(k)是否满足限幅要求：

其中，v(k-1)为k-1时刻的线速度，Δt为前后两次速度计算的时间间隔，a_tv为预设的速度变化率阈值，v_tv为预设的速度阈值；

若线速度v(k)满足限幅要求则向跟随平衡车输出线速度v(k)；否则向跟随平衡车输出速度阈值v_tv。