机械臂运动学建模方法（D-H）

发表于 2018-12-25

##建议

机械臂建模一般采用经典的Denavit-Hartenberg(DH) parameters方法。本文同时介绍另一种转换矩阵推导方法，以供对D-H参数识别有困难的同学进行机械臂建模。
研究规划算法方向的同学，推荐Steven M. LaValle的”Planning Algorithm”一书。
##切入正题
###1.建立关节坐标系
Oi：关节点；
Zi：关节角转动的轴；
Xi：在Zi于Zi-1

微分代数方程组（DAE）MATLAB解法

发表于 2018-11-18 | 更新于 2019-05-08

背景关联

空间机械臂通过计算关节指令实现自身运动，同时会对基座产生扰动。根据航天器与机械臂的运动学方程，以及组合体满足线动量和角动量守恒条件，可以列出一个既包含运动学微分方程，同时满足动量守恒条件的方程组，即微分代数方程组（differential algerbic equation）

切入正题

这类方程形式复杂，很难得到解析解，通常采用数值解法计算。MATLAB中的odes求解器可以求解常微分方程组，也可以解决本文中的DAE问题。

相较于常微分方程解法，DAE只需要增加一个mass参数即可，这个参数的功能是将方程组中的等式约束条件(即代数方程)，转换成微分方程的形式。

算例

DAE方程组为：

$\left\{\begin{matrix}y_{1}'=-0.04y_{1}+10^{4}y_{2}y_{3} \\ y_{2}'=0.04y_{1}-10^{4}y_{2}y_{3}-(3\times 10^{7})y_{2}^{2} \\ y_{1}+y_{2}+y_{3}=1 \end{matrix}\right.$

设置Mass属性M矩阵：

$M=[1,0,0;0,1,0;0,0,0]$

其含义为：

$My{}'=f(y)$

接下来就可以使用ode15s或者ode23s进行解算~

代码

dae_test.m

1
2
3
4
5
function dy=dae_test(t,y)
dy(1)=-0.04*y(1)+1e4*y(2)*y(3);
dy(2)=0.04*y(1)-1e4*y(2)*y(3)-3e7*y(2)*y(2);
dy(3)=y(1)+y(2)+y(3)-1;
dy=dy';

main.m

1
2
3
4
5
6
7
8
9
10
11
12
clc;clear;close;
y0=[1;0;0];
tspan = [0 4*logspace(-6,6)];
M=[1,0,0;
0,1,0;
0,0,0];
options=odeset('mass',M,'RelTol',1e-4,'AbsTol',[1e-6 1e-10 1e-6]);
[t,y]=ode15s(@dae_test,tspan,y0,options);
y(:,2) = 1e4*y(:,2);
semilogx(t,y);
ylabel('1e4 * y(:,2)');
title('DAE problem solved by ode15s');

仿真结果

Hi!

发表于 2018-07-24 | 更新于 2018-10-24

Hey everyone! This is Xiaochou‘s website for sharing some little skills!And welcome to comunicate with me if you are interested in anything about this!

隐枚举法

发表于 2018-07-22 | 更新于 2018-10-24

背景关联

隐枚举法是解决0-1整数规划问题的算法。课题组在研究无人机协同任务规划时，利用隐枚举法解决多无人机的任务分配问题。

0-1规划是整数线性规划的一种特殊情形，指变量只能取0或者1的规划问题。整数规划（Integer Programming）分为纯整数线性规划（所有变量为整数）、混合整数线性规划（部分变量必须取整数）、以及0-1型整数线性规划。目前流行的求解整数规划的方法大多针对整数线性规划。求解方法包括分枝定界法、割平面法、隐枚举法，还有蒙特卡洛方法（蒙特卡洛好神奇，啥都能干），等。

切入正题

首先将任务分配问题转换为0-1整数规划模型，这里举最简单的情况。

假设我有m（取m=4）架无人机执行n（取n=2）项不同任务，则变量总数为m*n=8，取 $x_{1}^{},$ $x_{2}^{},$ $x_{3}^{},$ $x_{4}^{}, x_{5}^{}, x_{6}^{,x_{7}^{}, x_{8}^{}, 其中 x_{1}^{} = 1 则表示飞机1执行任务1,\dots，以此类推， x_{8}^{} = 1 则表示飞机4执行任务2。不同无人机-任务对之间的执行时间用}$

$a_{n}^{}$ 表示，要求总代价时间最小：写出总的代价函数，如：
min a1x1+a2x2+a3x3+a4x4+a5x5+a6x6+a7x7+a8x8

增加约束条件：（如油耗、威胁等价等，还有保证任务被完成、一架飞机只能完成一个任务等）
s.t. Aeq X <= Beq && A x = B

接下来，就要用到隐枚举法来解决这个约束0-1整数规划问题了。

隐枚举算法过程分两步：1、预处理 2、隐枚举

预处理

（1）    将目标函数统一为求最小值的形式，即“max”变为“min”；
（2）    将约束条件不等式符号变为“>=”；
（3）    将目标函数中系数为负的变量转换为系数为正的变量；
（4）    按照变量系数递增的顺序重新排列目标函数中变量的顺序，同时约束条件中变量顺序对于排列。

预处理完毕，作用是：经过上述步骤得到的目标函数和约束条件使得我们可以按照二进制顺序开始枚举，由于目标函数求最小值，因此从目标函数的下界（x=0）开始，就能尽早找到可行解。

隐枚举

找到一个可行解z0后：

若之后寻求的目标函数z1大于该可行解z0，则一定不会是最优解，省去了验证约束条件的步骤，可以直接进行“剪枝”。“剪枝”的意思是：在之后的枚举过程中，直接略去包含当前变量取值子集的情况。
若之后寻求的目标函数z1小于可行解z0，则验证约束条件：若不满足，则继续枚举；若满足，则更新当前可行解，使z0=z1。

至此，用隐枚举解决任务分配模型的过程就结束了。代码是本人16年初的蜜汁风格（汗），就不贴了。最后，推荐大家两个计算规划问题的软件：CPLEX、LINGO，当然MATLAB也很好用。