引言

开局一张图

我来解释下图中线的含义，这个实际问题可以抽象为这样的模型：

圆柱表示机器人一个关节；

蓝色的线表示已经得到的机器人轴向方向，这里把它定义为z轴；

橙色的线表示空间中一个已经识别到的方向；

我们需要求解机器人这个关节的x轴方向

同时这里有两个必要的条件：

1、x轴一定在关节的面上；

2、我们希望x轴与空间中已经识别的橙色线共面。

分析

基于这两个要求，我们可以进一步分析：

1、x轴在面上，也就是说必须要满足z轴和x轴垂直的关系，也就是说需要点积等于0；

2、希望x轴与橙色线共面，也就是说需要x轴在橙色线上的投影最大。也就是说我们想要的是图中的绿色线而不是黄色线。

所以可以得到如下带约束优化问题：

建模成功后，便可以用matlab进行求解。

matlab

由于是非线性约束的优化问题，因此选用函数fmincon。

主函数

clear
lb=[-inf,-inf,-inf];
ub=[inf,inf,inf];
x0=[0,0,0];
A=[];b=[];Aeq=[];beq=[];
d1 = rand
d2 = 0+(1-d1-0)*rand
d3 = sqrt(1-d1^2-d2^2)
[x,fval,exitflag,output,lambda] =fmincon('fun1',x0,A,b,[d1,d2,d3],0,lb,ub,'fun2')

fun1为希望最小化的函数（加负号即可）；fun2为非线性约束。在fun1里，为了使橙色线不变，加了随机种子

rng(0)
f1 = rand
f2 = 0+(1-f1-0)*rand
f3 = sqrt(1-f1^2-f2^2)
f = -x(1)*f1-x(2)*f2-x(3)*f3;   

结果

如果上图看不清楚，那么下面这个图呢

三线共面，说明得到了正确的结果，再看看是否满足约束：

满足约束。

May the force be with you！