220g格斗机器人建造日志(3) —— 软件算法

再硬也不能解决所有问题，硬完了之后终究还是要软的。

硬件问题解决了之后就是漫长的调试软件工作了。整个车一共4个自由度，底盘两个，上层2个。2个定时器8个通道全占满了。且这些自由度的驱动方式都不一致，互相独立又要求协同工作，因此操作系统是必然选择。在os的大框架下，一共4个主要任务，分别是接收遥控器信息，然后控制底盘运动+云台+舵机臂。整体也不复杂，主要是调参挑了很久很久。

1. 操作系统

操作系统用的就是原子的ucosiii，没啥好说的。本来在stm32上面跑的代码原封不动下到at32里面完全正常运行，和stm一点不差，软件硬件外设全部完美兼容，简直就是偷懒的最佳选择。系统里面分了4个主要的任务，一个用来和遥控器通信，另外3个分别控制底盘和云台和机械臂，分工明确，互不干扰。4个软件定时器，1个用来定时读取陀螺仪dmp数据，因为没做陀螺仪中断；然后还一个用来运行舵机的闭环函数；余下的两个用来跑云台双闭环。为啥不把底盘控制也统一放定时器里呢，因为当初写的时候偷懒，就这么延续下来了。。。后期还考虑增加一些辅助功能，比如检测车身状态什么的。

2. 底盘控制

底盘的控制逻辑很简单，遥控器的输入+陀螺仪的状态，到底盘电机的输出。写了2种模式：有头模式和无头模式。

有头模式

云台定死不转，整车就像一台普通举升一样运行，铲子固定位于车体前方。遥控器摇杆对应车头运动的方向矢量。比如摇杆向左前方30°掰动，车头就会朝着左前方30°开；摇杆拉到右边，车身就会转向车头朝右开，模拟用根绳子牵着车头跑的那种感觉。车头的位置就设在铲子的地方，所以相当于直接控制铲子往哪个方向怼。这种控制方法按理说可以用于任何前后不对称的车型，而且武器攻击点距离轮轴越远效果就越好。本来是给偏横准备的，没想到先用在这儿了。
实现起来主要是运动学解算+几个坐标变换，最终目的是把摇杆水平和竖直方向的输入转化成轮子的输出。之前在网上看到一篇二轮差速AGV的运动学建模，

常见的几种AGV小车运动学建模

就套用了那个模型写了第一套转换的算法，看起来是可行的，但是没有实际下地跑过。这个模型就是需要的坐标变换比较复杂，后来转念一想，好像之前做麦轮运动控制的时候也没考虑什么模型之类的，直接用叠加原理就行了。于是果断抛弃了这个运动学建模，直接简单粗暴的把差速车的运动分解为两侧轮子共模的直线运动和两侧轮子差模的原地转向，然后二者叠加。放到模型里面看，就是我不需要知道共模速度Vx和差模速度是怎么来的，只需要把遥控器输入对应上去就行了。试下来效果居然挺不错，于是整个控制流程就定下来变成这样：

1. 遥控器摇杆极坐标输入对地运动信息（速率和方向），坐标变换转为车身极坐标输入（武器轴为极点），这一步是把遥控器输入相对固定地面的的绝对运动信息变成相对车身的相对运动信息，差不多就是脑子里转不过来的那部分。

//地面极坐标转为车身武器轴为极点的极坐标，车身等腰三角形模型，角度左-右+
//inRS[2]{Vm1,θ1}->outPS[2]{Vm2,θ2}
//需要用到车身角度
void Co_Trans_GND2Bot_PS(float inPS[2],float outPS[2],float yawAngle)
{
	outPS[0] = inPS[0];
	outPS[1] = inPS[1]-yawAngle;
	if(outPS[1] > 180)
	{
		outPS[1] = outPS[1] - 360;
	}
	else if(outPS[1] < -180)
	{
		outPS[1] = outPS[1] + 360;
	}
}

2. 车身极坐标转为车身直角坐标。这一步的目的是分离与车身前进方向平行的直线运动分量和与车身前进方向正交的转动运动分量。直线运动靠两侧轮子输入共模运动即可实现，转动运动则是两侧轮子差模运动，二者叠加就是车身的运动。

//车身极坐标转为车身直角坐标（武器轴为原点，主要是分离平动和转动分量），车身等腰三角形模型，角度左-右+
//inPS[2]{Vm,θ}->outRS[2]{Vx,Vy}
void Co_Trans_Bot_PS2RS(float inPS[2],float outRS[2])
{
	inPS[1] = inPS[1] * PI/180;
	outRS[1] = inPS[0]*cos(inPS[1]);
	outRS[0] = inPS[0]*sin(inPS[1]);
}

3.这样算出来的out输出就是两侧轮子的共模和差模运动了，按合适的系数叠加一下就ok了。

这个样子的操作显然更加符合个人习惯，要不然如果用那种传统的前进后退+左右转的控制方式，脑子根本反应不过来的。

无头模式

这个模式其实才是云台的本意，把底盘和云台的运动独立开来，底盘可以全向运动，云台举升臂的朝向和车身运动方向无关，类似于rm战车的思路。这个就要求车身和云台都必须保持快速响应，并且必须有相应配合。本来以为控制算法随便调调就行，没想到这么几个玩意花了整整两个月才搞完。不得不感谢之前写过的超级无敌什么什么pid算法库，要没有那玩意自己一个个去写pid函数都是个灾难。

车身只有一个角度环，遥控器以极坐标方式输入速率和方向，闭环反馈为陀螺仪偏航角度，输出直接加到电机pwm上。按理说中间缺了一个电机速度环，导致调的时候相当费劲。一开始用的专家pid，参数多的一批还难调，后来改成普通pid稍微好了一点，加上变速积分，效果不错。最后还是叠加的思路，速率对应共模输入，方向经闭环算法算出差模输入，根据当前车身角度与期望角度的差值判断共模和差模叠加的系数。 差的多就把差模的系数放大点，差的少就把共模的系数放大。这样子调出来的效果就是底盘的运动方向对应遥控器摇杆，摇杆往那边掰车身就往哪个方向跑。基本上挺灵活的。

3. 云台

云台用一个n20来带，说实话做出来之后才发觉这样子好像有点脆。而且找遍全网也只发现空载500rpm的减速机，带编码器的没有更快的了。最坑的一点就是，带编码器的n20输出轴上的齿轮，比不带编码器的n20大了一点点！！ 虽然普通的减速器换上去也能啮合，但是阻力大了很多，相当于减速器自己成了负载，运行一会儿电机就滚烫了。没有办法只能用编码n20适配的减速箱。这就导致万一这个减速箱被打坏了就只能换整个电机。坑死。

本来云台也只想用一个角度环搞定的，后来发现n20本身线性也不怎么好，单个n20如果只有位置环的话效果很差。不得已又加了一层速度环，效果一下子好了不少，响应也快了，甚至快过头了，让他跟随车身运动的时候一卡一卡的。一开始还不知道怎么回事，在那拼命调参，后来网上找了几篇云台的资料才意识到可能跟控制频率有关。
一个机械系统，反馈的频率通常要远远高于控制频率，否则就相当于没有反馈；然后外环位置环的频率也要比内环速度环的频率高才行。机械系统的带宽大概几十Hz，控制频率需要是系统带宽的5倍以上。 那么位置环算80Hz，角度环160Hz，陀螺仪用的dmp库，解算频率最高200Hz，基本符合要求了。之前控制频率太快了，导致反馈失效。把控制频率降下来，效果反而好了。

4. 舵机

舵机一开始也是自己随便写的pid算法，后来索性也统一改成用现成的变积分，否则在0位的时候效果很差。应该是舵机减速比太大了，静差很难消掉。如果用普通pid的话静差有接近20°，非常的夸张。改成变积分pid也有不小的静差难以根除，但是实在不想再加一个串级了，会累死的。。。。。

5. 通信

自己把串口通信稍微包装了一下，加了帧头识别防止误传，主要是几个连续自由度和按键开关的识别，其他没啥。底盘和云台基本功能有了之后剩下的就是加一些辅助操作，比如切换控制模式什么的，后面慢慢加吧，再后面的跟别人约完打几场看看效果再决定改进方向。

从构思开始历时一年，终于堪堪凑出一台基本能用的车，不得不说自己真是太懒惰了。。。但是另一个方面看，车做出来一场没打过，倒是已经啪啪啪更新了三个版本了，并且还将继续更新下去。只能说理想和现实之间的差距犹如天渊之隔，一步一步才能越来越近。想起胡歌在半决赛之后说的，——How did you get here？——One step at a time. Sawblaze也是从第二季开始每季都在更新都在进步，每季都比前一次更加接近桂冠，历六载终成圣。这注定会是一条亘古绵长的路，无所谓启程，亦无人知晓终点，唯有前行方能找到答案。
——Don‘t dream，make it real。