本发明涉及星球探测平台控制技术领域，具体涉及一种松软崎岖地形中摇杆摇臂式星球车轨迹跟踪协调控制方法。
背景技术：
星球车在松软崎岖的星球表面执行探测任务时，其运动控制器是整个系统的核心单元之一，是决定能否顺利完成探测任务的根本。摇杆摇臂式的星球车通常采用多轮驱动的设计构型，属于典型的冗余控制系统，在松软崎岖地形中运动时，地形不同，各车轮运动状态不同，极易在同侧车轮之间产生推挤和牵拉现象，造成各车轮之间存在内力损耗，进而消耗能量。同时松软地形中，侧滑和滑转现象的存在，导致星球车存在偏离期望轨迹的问题。以保证星球车能够跟踪规划的期望轨迹并且在运行过程中尽可能的降低同侧车轮之间的内力为主要目标，运动控制器的设计重点关注轨迹跟踪与协调分配的同时实现。
目前，国内外的星球车控制通常将两个问题分别进行研究，没有有效的解决将两个任务目标同时实现的问题，因此如何设计一个控制器，在实现轨迹跟踪控制的同时，能够有效地降低同侧车轮之间的内力，是目前星球车运动控制研究的一项挑战。
技术实现要素：
本发明为了克服以上技术的不足，提供了一种保证轨迹跟踪和控制输入协调分配同时实现的松软崎岖地形中摇杆摇臂式星球车轨迹跟踪协调控制方法。
本发明克服其技术问题所采用的技术方案是：
一种松软崎岖地形中摇杆摇臂式星球车轨迹跟踪协调控制方法，包括如下步骤：a)定义星球车在世界坐标系中的位置信息为(x,y,z)，星球车的横滚角为俯仰角为ψ，偏航角为θ，以车体前进方向为x轴正方向，以垂直于车体向上为z轴正方向，根据右手定则建立车体坐标系(xb,yb,zb)，车体形心在车体坐标系中的速度为(vbx,vby,vbz)，通过公式建立车体三维运动学模型，其中q为位置向量，q＝(x,y,z)，为q的求导运算结果，ωbz为星球车绕车体坐标系z轴旋转角速度，δ0为系统的外部干扰值；
b)根据公式建立星球车整车动力学模型，式中m′＝diag(m,m,ibz)，m为星球车质量，ibz为车体绕车体坐标系z轴旋转的转动惯量，为车体坐标系中x方向速度、y方向速度和绕z轴旋转的角加速度构成的加速度的向量，g为重力项矢量，u为模型控制输入矢量；c)根据步骤a)中的车体三维运动学模型带入步骤b)中的星球车整车动力学模型得到基于三维运动学信息的动力学模型；
d)定义星球车的期望位置为qd，系统误差为定义性能指标为z∞、z2，根据公式建立针对星球车整车动力学模型的误差增广系统，式中为emix的求导运算结果，性能指标z∞、z2为整车控制输入umix＝kmixemix，kmix为状态反馈矩阵，amix为系统参数矩阵，dmix为干扰项，bmix为输入参数矩阵；
e)通过求解qp优化问题，计算得到最优的各车轮的力组成的力矢量；
f)通过公式建立速度模式下期望力跟踪模型，式中ftwi为星球车第i个车轮驱动力，为ftwi的求导运算结果，iwi为星球车第i个车轮转动惯量，rwi为星球车第i个车轮等效半径，ωwi为星球车第i个车轮旋转角速度，为ωwi的二阶导运算，fw(fnwi,swi)为与星球车第i个车轮法向支撑力fnwi和滑转率swi相关的总和扰动项；
g)根据公式建立线性自抗扰控制器，式中xfintd1和xfintd2为输入的跟踪微分器的输出信号，为xfintd1的求导运算结果，xfouttd1和xfouttd2为输出端的跟踪微分器的输出信号，为xfouttd1的求导运算结果，xfleso1和xfleso2为扩张状态观测器的估计状态参数，为xfleso1的求导运算结果，为xfleso2的求导运算结果，fhfinput和fhfoutput为跟踪微分器的最速跟踪控制函数，βf1和βf2为状态反馈控制律的调节参数，αf1、αf2、δf1、δf2为状态反馈控制律的调节参数，eleso、ef1、ef2为跟踪误差，ufx0为状态反馈控制律的计算输出，ufx为为状态反馈控制律的控制器输出，kfp1、kfp2、b为输入增益；h)通过线性自抗扰控制器控制星球车转速输入，得到第i个车轮控制输入ωwi，i＝{1,2,...,6}。
进一步的，步骤c)中通过公式建立基于三维运动学信息的动力学模型，式中为q的二阶导运算，m＝m′at，t为转置运算，为哥氏力项，为a的求导运算结果，d为干扰项，为b的求导运算结果，为vbz的求导运算结果。
进一步的，步骤e)中根据公式计算最优的各车轮的力组成的力矢量fw，式中qopt、popt为优化问题的参数矩阵，aeq为等式约束的参数矩阵，i3为3×3的单位矩阵，为星球车的6个车轮坐标系与车体坐标系之间的旋转矩阵，bpwi为第i个车轮到车体中心的距离向量，i＝{1,2,…,6}，beq为约束条件矩阵，式中umixx、umixy、umixθ分别为整车控制输入umix在x,y,θ上的分量，gbz为重力沿z轴方向的分力，ibx、iby分别为车体绕x轴和绕y轴的旋转惯量，分别为车体绕x轴和绕y轴旋转的角加速度，aie为不等式约束的参数矩阵，bie为不等式约束的上限条件向量，1≤i≤6，fmaxi为星球车第i个车轮最大驱动力上限，i＝{1,2,…,6}。本发明的有益效果是：基于运动学和动力学分析设计基于三维运动学信息的动力学模型，然后基于此模型设计误差增广系统，并基于系统模型设计混合h2/h∞-qp控制器，通过分层求解优化问题，得到整车控制输入和车轮控制输入，并通过速度模式下的力跟踪控制器来实现车轮转速控制输入的计算，最终通过该输入实现在轨迹跟踪的过程中保证跟踪误差稳定在可接受范围内的同时，协调分配各车轮的控制输入，降低同侧车轮之间的内力损耗。
附图说明
图1为本发明的星球车的结构示意图；
图2为本发明的星球车车体坐标系和世界坐标系速度示意图；
图3为本发明的星球车在松软崎岖地形上的受力分析示意图；。
具体实施方式
下面结合附图1至附图3对本发明做进一步说明。
一种松软崎岖地形中摇杆摇臂式星球车轨迹跟踪协调控制方法，包括如下步骤：a)定义星球车在世界坐标系中的位置信息为(x,y,z)，星球车的横滚角为，俯仰角为ψ，偏航角为θ，以车体前进方向为x轴正方向，以垂直于车体向上为z轴正方向，根据右手定则建立车体坐标系(xb,yb,zb)，车体形心在车体坐标系中的速度为(vbx,vby,vbz)，通过公式建立车体三维运动学模型，其中q为位置向量，q＝(x,y,z)，为q的求导运算结果，ωbz为星球车绕车体坐标系z轴旋转角速度，δ0为系统的外部干扰值；
b)根据公式建立星球车整车动力学模型，式中m′＝diag(m,m,ibz)，m为星球车质量，ibz为车体绕车体坐标系z轴旋转的转动惯量，为车体坐标系中x方向速度、y方向速度和绕z轴旋转的角加速度构成的加速度的向量，g为重力项矢量，u为模型控制输入矢量；
c)根据步骤a)中的车体三维运动学模型带入步骤b)中的星球车整车动力学模型得到基于三维运动学信息的动力学模型；
d)定义星球车的期望位置为qd，系统误差为定义性能指标为z∞、z2，根据公式建立针对星球车整车动力学模型的误差增广系统，式中为emix的求导运算结果，性能指标z∞、z2为整车控制输入umix＝kmixemix，kmix为状态反馈矩阵，amix为系统参数矩阵，dmix为干扰项，bmix为输入参数矩阵；
e)通过求解qp优化问题，计算得到最优的各车轮的力组成的力矢量；
f)通过公式建立速度模式下期望力跟踪模型，式中ftwi为星球车第i个车轮驱动力，为ftwi的求导运算结果，iwi为星球车第i个车轮转动惯量，rwi为星球车第i个车轮等效半径，ωwi为星球车第i个车轮旋转角速度，为ωwi的二阶导运算，fw(fnwi,swi)为与星球车第i个车轮法向支撑力fnwi和滑转率swi相关的总和扰动项；
g)根据公式建立线性自抗扰控制器，式中xfintd1和xfintd2为输入的跟踪微分器的输出信号，为xfintd1的求导运算结果，xfouttd1和xfouttd2为输出端的跟踪微分器的输出信号，为xfouttd1的求导运算结果，xfleso1和xfleso2为扩张状态观测器的估计状态参数，为xfleso1的求导运算结果，为xfleso2的求导运算结果，fhfinput和fhfoutput为跟踪微分器的最速跟踪控制函数，βf1和βf2为状态反馈控制律的调节参数，αf1、αf2、δf1、δf2为状态反馈控制律的调节参数，eleso、ef1、ef2为跟踪误差，ufx0为状态反馈控制律的计算输出，ufx为为状态反馈控制律的控制器输出，kfp1、kfp2、b为输入增益；h)通过线性自抗扰控制器控制星球车转速输入，得到第i个车轮控制输入ωwi，i＝{1,2,…,6}。
步骤c)中通过公式建立基于三维运动学信息的动力学模型，式中为q的二阶导运算，m＝m′at，t为转置运算，为哥氏力项，为a的求导运算结果，d为干扰项，为b的求导运算结果，为vbz的求导运算结果。
基于运动学和动力学分析设计基于三维运动学信息的动力学模型，然后基于此模型设计误差增广系统，并基于系统模型设计混合h2/h∞-qp控制器，通过分层求解优化问题，得到整车控制输入和车轮控制输入，并通过速度模式下的力跟踪控制器来实现车轮转速控制输入的计算，最终通过该输入实现在轨迹跟踪的过程中保证跟踪误差稳定在可接受范围内的同时，协调分配各车轮的控制输入，降低同侧车轮之间的内力损耗。
步骤e)中根据公式计算最优的各车轮的力组成的力矢量fw，式中qopt、popt为优化问题的参数矩阵，aeq为等式约束的参数矩阵，i3为3×3的单位矩阵，为星球车的6个车轮坐标系与车体坐标系之间的旋转矩阵，bpwi为第i个车轮到车体中心的距离向量，i＝{1,2,…,6}，beq为约束条件矩阵，式中umixx、umixy、umixθ分别为整车控制输入umix在x,y,θ上的分量，gbz为重力沿z轴方向的分力，ibx、iby分别为车体绕x轴和绕y轴的旋转惯量，分别为车体绕x轴和绕y轴旋转的角加速度，aie为不等式约束的参数矩阵，bie为不等式约束的上限条件向量，1≤i≤6，fmaxi为星球车第i个车轮最大驱动力上限，i＝{1,2,…,6}。