问:
How to tune the PID parameters using Fuzzy Logic?
I previously used the Ziegler method to tune the parameters of my PID controller to control my robot's position. I then implemented fuzzy logic for self-tuning the parameters.
I have two inputs to the fuzzy logic controller; one is the position error and the error rate.

I know that my problem might be due to not understanding the effect of each parameter very well.

The problem is that I am confused in setting up the fuzzy rules. When do I need to use high and low values for Kp, Kd and Ki to achieve the best tuning? Is it that Kp must be very low when the error is almost zero (hence, the robot is at the desired position)? The same question applies for all of the three parameters.

答:
The paper Controlling of Quadrotor UAV Using a Fuzzy System for Tuning the PID Gains in Hovering Mode by E. Abbasi, M. J. Mahjoob explains how to tune PID gains with fuzzy logic. You can find many papers about singleton tuning but this paper shows totally fuzzy control

find PID gains with ziegler-nichols (or another technique)

Create a fuzzy PID gain changer which has inputs error (e) and change in error(de)

Define fuzzification graphs for inputs and outputs. Define limits (also you can change the shape) like

name       [min,peak,max]

very small [-1,-1,-0.6], small [-1,-0.6,0], medium [-0.6,0,0.6], big [0,0.6,1], very big [0.6,1,1]

create rules like

if **e** and/or **de** *fuzzyname* (small,big etc.) than KI is fuzzyname (small,big etc.)

Defuzzyfy the result.

You can use tools like matlab fuzzy toolbox or python skfuzzy

The tipping problem can be used as Fuzzy-PID just change qualtiy as e and service as de and lastly you can change tip output as KP/ KI/ KD
(there is example about tipping problem: python scikit fuzzy - Fuzzy Control Systems: The Tipping Problem)

Note 1: Error ranges should be well defined so you must log the error and change in error. The limits must be in max and min values of these values

Note 2: The output value range is good between -1 and 1.

An example code for Fuzzy-PID in python is here:

```<code># -*- coding: utf-8 -*-
"""
@author: acs
"""

import skfuzzy as fuzz
from skfuzzy import control as ctrl
import acspid
import numpy as np
from matplotlib import pyplot as plt

plt.ion()
fig=plt.figure()

ferr = ctrl.Antecedent(np.arange(-150, 150, 1), 'ferr')
fder = ctrl.Antecedent(np.arange(-150, 150, 1), 'fder')
fout = ctrl.Consequent(np.arange(-1, 1, 0.01), 'fout')

ferr.automf(5)
fder.automf(5)
fout.automf(5)
fout['poor'] = fuzz.trimf(fout.universe, [-1, -1, -0.5])
fout['mediocre'] = fuzz.trimf(fout.universe, [-1, -0.5, 0])
fout['average'] = fuzz.trimf(fout.universe, [-0.1, 0, 0.1])
fout['decent'] = fuzz.trimf(fout.universe, [0, 0.5, 2])
fout['good'] = fuzz.trimf(fout.universe, [0.5, 1, 1])
fout.view()
ferr.view()
fder.view()
plt.show()
plt.pause(0.0001)

#'poor'; 'mediocre'; 'average'; 'decent', or 'good'
rules=[]
rules.append(ctrl.Rule(ferr['average'] | fder['average'] , fout['average']))
rules.append(ctrl.Rule(ferr['decent'] | fder['decent'] , fout['decent']))
rules.append(ctrl.Rule(ferr['good'] | fder['good'] , fout['good']))
rules.append(ctrl.Rule(ferr['mediocre'] | fder['mediocre'] , fout['mediocre']))
rules.append(ctrl.Rule(ferr['poor'] | fder['poor'] , fout['poor']))

fctrl = ctrl.ControlSystem(rules)
fpid = ctrl.ControlSystemSimulation(fctrl)

pid=acspid.pidcont(1.2,0.02,0.01,5,-5)

pid2=acspid.pidcont(1.2,0.02,0.01,5,-5)

d=np.zeros(10)
for i in range(10):
    d=np.append(d,np.ones(10)*np.random.uniform(-100,100,1))

print len(d)
m=[]
m.append(0.0)
m2=[]
m2.append(0.0)
e=[]
de=[]
e2=[]
de2=[]

kp=pid.kp
kd=pid.kd
ki=pid.ki
for i in range(len(d)):
    pid.setDesired(d[i])
    print "e:",pid.error ,"\t de:", pid.ed
    fpid.input['ferr'] = pid.error
    fpid.input['fder'] = pid.ed
    fpid.compute()
    newpid=np.abs(fpid.output['fout'])
    print "PID:", newpid*pid.kp,"\t",newpid*pid.ki,"\t",newpid*pid.kd
    pid.setGains(newpid*kp,newpid*ki,newpid*kd)
    newm=pid.update(m[-1])
    newm=m[-1]+newm
    print i,m[-1],newm
    m.append(newm)
    e.append(pid.error)
    de.append(pid.ed)

    pid2.setDesired(d[i])
    newm2=pid2.update(m2[-1])
    newm2=m2[-1]+newm2
    m2.append(newm2)
    e2.append(pid2.error)
    de2.append(pid2.ed)

    ax1 =plt.subplot(2,1,1)
    ax1.set_xlim([0, len(d)])
    ax1.set_ylim([-200, 200])
    plt.grid()
    plt.plot(range(len(m)),m,linewidth=5.0)
    plt.plot(range(len(m2)),m2,linewidth=2.0)
    plt.plot(range(len(d)),d,'g--')

    plt.title('Status')
    ax2=plt.subplot(2,1,2)
    ax2.set_xlim([0, 50])
    ax2.set_ylim([-100, 100])
    plt.plot(range(len(e)),e,'r-',range(len(de)),de,'g-')
    plt.grid()
    plt.title('e and ed')
    #plt.draw()
    plt.show()
    plt.pause(0.0001)
</code>```

Fuzzy input membership functions:

Fuzzy output Membership function:

Status: In the status plot dashed line is target value, red line is PID and green line is Fuzzy-PID

Here the acspid class

```<code>class pidcont():
    def __init__(self,P,I,D,pmax,pmin):
        self.kp=P
        self.kd=D
        self.ki=I
        self.pidmax=pmax
        self.pidmin=pmin
        self.desired=0.0
        self.error=0.0
        self.elast=0.0
        self.esum=0.0
        self.eder=0.0
    def update(self,current):
        self.error=self.desired-current
        self.eder=self.error-self.elast
        self.elast=self.error
        self.esum=self.esum+self.error
        if self.esum>self.pidmax:
            self.esum=self.pidmax
        elif self.esum<self.pidmin:
            self.esum=self.pidmin

        self.P=self.kp*self.error
        self.D=self.kd*self.eder
        self.I=self.ki*self.esum
        pid=self.P+self.I+self.D
        return pid
    def setDesired(self,d):
        self.desired=d
    def setGains(self,P,I,D):
        self.kp=P
        self.kd=D
        self.ki=I
    def setLimits(self,pmax,pmin):
        self.pidmax=pmax
        self.pidmin=pmin
</code>```

Body:
The paper Controlling of Quadrotor UAV Using a Fuzzy System for Tuning the PID Gains in Hovering Mode by E. Abbasi, M. J. Mahjoob explains how to tune PID gains with fuzzy logic. You can find many papers about singleton tuning but this paper shows totally fuzzy control

find PID gains with ziegler-nichols (or another technique)

Create a fuzzy PID gain changer which has inputs error (e) and change in error(de)

Define fuzzification graphs for inputs and outputs. Define limits (also you can change the shape) like

name       [min,peak,max]

very small [-1,-1,-0.6], small [-1,-0.6,0], medium [-0.6,0,0.6], big [0,0.6,1], very big [0.6,1,1]

create rules like

if **e** and/or **de** *fuzzyname* (small,big etc.) than KI is fuzzyname (small,big etc.)

Defuzzyfy the result.

You can use tools like matlab fuzzy toolbox or python skfuzzy

The tipping problem can be used as Fuzzy-PID just change qualtiy as e and service as de and lastly you can change tip output as KP/ KI/ KD
(there is example about tipping problem: python scikit fuzzy - Fuzzy Control Systems: The Tipping Problem)

Note 1: Error ranges should be well defined so you must log the error and change in error. The limits must be in max and min values of these values

Note 2: The output value range is good between -1 and 1.

An example code for Fuzzy-PID in python is here:

# -*- coding: utf-8 -*-
"""
@author: acs
"""

import skfuzzy as fuzz
from skfuzzy import control as ctrl
import acspid
import numpy as np
from matplotlib import pyplot as plt

plt.ion()
fig=plt.figure()

ferr = ctrl.Antecedent(np.arange(-150, 150, 1), 'ferr')
fder = ctrl.Antecedent(np.arange(-150, 150, 1), 'fder')
fout = ctrl.Consequent(np.arange(-1, 1, 0.01), 'fout')

ferr.automf(5)
fder.automf(5)
fout.automf(5)
fout['poor'] = fuzz.trimf(fout.universe, [-1, -1, -0.5])
fout['mediocre'] = fuzz.trimf(fout.universe, [-1, -0.5, 0])
fout['average'] = fuzz.trimf(fout.universe, [-0.1, 0, 0.1])
fout['decent'] = fuzz.trimf(fout.universe, [0, 0.5, 2])
fout['good'] = fuzz.trimf(fout.universe, [0.5, 1, 1])
fout.view()
ferr.view()
fder.view()
plt.show()
plt.pause(0.0001)

#'poor'; 'mediocre'; 'average'; 'decent', or 'good'
rules=[]
rules.append(ctrl.Rule(ferr['average'] | fder['average'] , fout['average']))
rules.append(ctrl.Rule(ferr['decent'] | fder['decent'] , fout['decent']))
rules.append(ctrl.Rule(ferr['good'] | fder['good'] , fout['good']))
rules.append(ctrl.Rule(ferr['mediocre'] | fder['mediocre'] , fout['mediocre']))
rules.append(ctrl.Rule(ferr['poor'] | fder['poor'] , fout['poor']))

fctrl = ctrl.ControlSystem(rules)
fpid = ctrl.ControlSystemSimulation(fctrl)

pid=acspid.pidcont(1.2,0.02,0.01,5,-5)

pid2=acspid.pidcont(1.2,0.02,0.01,5,-5)

d=np.zeros(10)
for i in range(10):
    d=np.append(d,np.ones(10)*np.random.uniform(-100,100,1))

print len(d)
m=[]
m.append(0.0)
m2=[]
m2.append(0.0)
e=[]
de=[]
e2=[]
de2=[]

kp=pid.kp
kd=pid.kd
ki=pid.ki
for i in range(len(d)):
    pid.setDesired(d[i])
    print "e:",pid.error ,"\t de:", pid.ed
    fpid.input['ferr'] = pid.error
    fpid.input['fder'] = pid.ed
    fpid.compute()
    newpid=np.abs(fpid.output['fout'])
    print "PID:", newpid*pid.kp,"\t",newpid*pid.ki,"\t",newpid*pid.kd
    pid.setGains(newpid*kp,newpid*ki,newpid*kd)
    newm=pid.update(m[-1])
    newm=m[-1]+newm
    print i,m[-1],newm
    m.append(newm)
    e.append(pid.error)
    de.append(pid.ed)

    pid2.setDesired(d[i])
    newm2=pid2.update(m2[-1])
    newm2=m2[-1]+newm2
    m2.append(newm2)
    e2.append(pid2.error)
    de2.append(pid2.ed)

    ax1 =plt.subplot(2,1,1)
    ax1.set_xlim([0, len(d)])
    ax1.set_ylim([-200, 200])
    plt.grid()
    plt.plot(range(len(m)),m,linewidth=5.0)
    plt.plot(range(len(m2)),m2,linewidth=2.0)
    plt.plot(range(len(d)),d,'g--')

    plt.title('Status')
    ax2=plt.subplot(2,1,2)
    ax2.set_xlim([0, 50])
    ax2.set_ylim([-100, 100])
    plt.plot(range(len(e)),e,'r-',range(len(de)),de,'g-')
    plt.grid()
    plt.title('e and ed')
    #plt.draw()
    plt.show()
    plt.pause(0.0001)

Fuzzy input membership functions:

Fuzzy output Membership function:

Status: In the status plot dashed line is target value, red line is PID and green line is Fuzzy-PID

Here the acspid class

class pidcont():
    def __init__(self,P,I,D,pmax,pmin):
        self.kp=P
        self.kd=D
        self.ki=I
        self.pidmax=pmax
        self.pidmin=pmin
        self.desired=0.0
        self.error=0.0
        self.elast=0.0
        self.esum=0.0
        self.eder=0.0
    def update(self,current):
        self.error=self.desired-current
        self.eder=self.error-self.elast
        self.elast=self.error
        self.esum=self.esum+self.error
        if self.esum>self.pidmax:
            self.esum=self.pidmax
        elif self.esum<self.pidmin:
            self.esum=self.pidmin

        self.P=self.kp*self.error
        self.D=self.kd*self.eder
        self.I=self.ki*self.esum
        pid=self.P+self.I+self.D
        return pid
    def setDesired(self,d):
        self.desired=d
    def setGains(self,P,I,D):
        self.kp=P
        self.kd=D
        self.ki=I
    def setLimits(self,pmax,pmin):
        self.pidmax=pmax
        self.pidmin=pmin


问:
Torque in kg/cm?
I was looking up the motor parameters for some stepper motor where they listed the torque of the motor at different current/voltage but the torque they listed was in kg/cm.

How is kg/cm even a remotely acceptable unit for torque?

How do I calculate the torque in Nm from kg/cm?

Clarity note: Its not kgcm which represents [0.098 kilogram force = 1 Nm.]

Website where this happens.

答:
```<code>Y[Nm]=X[kg-cm]*9.81[m/s^2]/100[cm]
</code>```

1kg multiplyed with gravitational constant will give you 10N and normalizing the cm to m by multiplying it with 0.01.

Basically, unit in their table is wrong. It is not kg/cm but rather kg-cm. Kg/cm would mean normalized linear pressure, which makes no sense for torque.

Also, what you have mentioned that 0.02 Nm is small torque. It is but not for 0.1A and 0.4V. If you take a look at torque for 1.5A, it is 0.18Nm for 9W of power.

ContentLicense: CC BY-SA 3.0

Title: Torque in kg/cm?

Body:
Y[Nm]=X[kg-cm]*9.81[m/s^2]/100[cm]

1kg multiplyed with gravitational constant will give you 10N and normalizing the cm to m by multiplying it with 0.01.

Basically, unit in their table is wrong. It is not kg/cm but rather kg-cm. Kg/cm would mean normalized linear pressure, which makes no sense for torque.

Also, what you have mentioned that 0.02 Nm is small torque. It is but not for 0.1A and 0.4V. If you take a look at torque for 1.5A, it is 0.18Nm for 9W of power.


问:
UR3 TCP/IP communication protocol
I'm currently programming a C socket for communicating via (Ethernet) TCP/IP with an UR3 robot which control software is version 3.3. I'm able to get raw data from the ports 30001,30002, and 30003. I can deserialize the data thanks to the source of a ROS driver. But I wish to have more information about deserialization, and Universal Robots seems to not provide such information. Do anybody knows where to get info about the TCP packets that the robot sends?</p>

Title: UR3 TCP/IP communication protocol

Body:
Based on the ports you have mentioned you are probably using URScript.
Are you aware of this documentation from the UR website, describing the ethernet packets?

The example given is

Lets say the client has recieved this data:

Received 465 bytes.
Bytes: [0] [0] [1] [209] [16] [0] [0] [0] [29] [0] [0] [0] [0] [0] [0] [5] [7] [246] [1] [1] [1] [0] [0] [0] [0] [0] [63] [240] [0] [0] [0] [0] [0] [0] [0] [0]
[0] [251] [1] [64] [1] [78] [244] [77] [189] [249] [149] [64] [1] [78] [247] [89] [95] [104] [85] [0] [0] [0] [0] [0] [0] [0] [0] [188] [220] [97] [3] [66] [62]
[0] [0] [66] [0] [102] [103] [66] [99] [153] [154] [253] [191] [246] [74] [170] [216] [242] [29] [102] [191] [246] [74] [178] [44] [92] [72] [137] [0] [0] [0] [0]
[0] [0] [0] [0] [192] [1] [106] [78] [66] [63] [153] [154] [66] [4] [204] [205] [66] [104] [0] [0] [253] [63] [253] [49] [202] [91] [202] [8] [64] [63] [253] [49]
[210] [233] [51] [16] [35] [0] [0] [0] [0] [0] [0] [0] [0] [191] [155] [135] [34] [66] [62] [0] [0] [65] [249] [153] [154] [66] [100] [0] [0] [253] [191] [220] [115]
[204] [104] [205] [239] [254] [191] [220] [118] [68] [109] [49] [34] [158] [0] [0] [0] [0] [0] [0] [0] [0] [190] [60] [245] [109] [66] [63] [153] [154] [66] [25] [51]
[51] [66] [116] [204] [205] [253] [63] [242] [146] [224] [105] [231] [66] [209] [63] [242] [146] [65] [3] [193] [196] [156] [0] [0] [0] [0] [0] [0] [0] [0] [190] [115]
[157] [190] [66] [62] [0] [0] [66] [21] [51] [51] [66] [119] [153] [154] [253] [191] [231] [207] [8] [215] [85] [22] [88] [191] [231] [206] [77] [130] [151] [190] [17]
[191] [146] [242] [158] [148] [114] [240] [57] [188] [224] [224] [96] [66] [68] [102] [103] [66] [37] [153] [154] [66] [127] [153] [154] [253] [0] [0] [0] [53] [4] [63]
[217] [153] [52] [224] [36] [238] [93] [191] [217] [153] [169] [67] [241] [211] [23] [63] [207] [255] [137] [8] [22] [253] [198] [63] [240] [0] [170] [111] [207] [54]
[176] [63] [243] [51] [88] [137] [58] [151] [217] [63] [201] [148] [119] [151] [70] [237] [237] [0] [0] [0] [29] [5] [64] [143] [64] [0] [0] [0] [0] [0] [64] [143] [64]
[0] [0] [0] [0] [0] [64] [143] [64] [0] [0] [0] [0] [0] [0] [0] [0] [61] [3] [0] [63] [0] [0] [0] [0] [63] [123] [129] [184] [27] [129] [184] [28] [63] [112] [225]
 [14] [16] [225] [14] [17] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [0] [66] [87] [51] [51] [66] [66] [0] [0] [62] [35] [215] [11] [61]
[241] [169] [253] [0] [0] [0] [37] [2] [0] [0] [63] [141] [83] [47] [180] [171] [196] [232] [63] [137] [46] [99] [102] [69] [149] [155] [66] [55] [51] [51] [0] [59]
[68] [155] [166] [66] [92] [0] [0] [253] [0]

So to start from the beginning:

[0],[0],[1],[209]: Length is 1*256+209 = 465

[16] Package type is "Robot State"

[0][0][0][29] First package length is 29

[0] Package type is 0: "Robot Mode Data"

[0] [0] [0] [0] [0] [5] [7] [246] Timestamp is 329718

[1] isRobotConnected True

[1] isRealRobotEnabled True

[1] isPowerOnRobot True

[0] isEmergencyStopped False

[0] isSecurityStopped False

[0] isProgramRunning False

[0] isProgrampaused False

[0] robot mode is 0

[63] [240] [0] [0] [0] [0] [0] [0] Speed fraction as a double                       see     http://en.wikipedia.org/wiki/Double-precision_floating-point_format                             , network byte order

[0] [0] [0] [251] Second package length is 251

[1] Second package type is "joint data"

[64] [1] [78][244] [77] [189] [249] [149] Joint 0 position

[64] [1] [78] [247] [89] [95] [104] [85] Joint 0 target position

[0] [0] [0] [0] [0] [0] [0] [0] Joint 0 target speed

[188] [220] [97] [3] Joint 0 current

[66] [62] [0] [0] Joint 0 voltage

[66] [0] [102] [103]  Joint 0 motor temperature

[66] [99] [153] [154] Joint 0 microprocessor temperature

[253] Joint 0 joint mode

[191] [246] [74] [170] [216] [242] [29] [102] Joint 1 position


问:
Can I create an autonomous robot that can adapt its strategy using machine learning?
BACKGROUND:

I am creating a robot to score the most points in 30 seconds while running autonomously. Naturally, two thoughts come to mind: Linear programming and machine learning.

Linear programming would provide a stable, simple method of scoring points. However, it is limited by what it can do, and optimizing scores would require reworking the entire bot.

PROBLEMS:

The robot and code itself can change, but it is not time efficient due to the fact that it would require completely reworking the code and robot by a single programmer. The robot has to work with what its got.

QUESTION:

Can I create Android Java classes to allow my robot to work the field as an AI and tweak its strategy or course of action based on stats from previous rounds? (Self-Supervised Learning) If it is possible to do, how would I do it?

SPECS:

Max robot size of 18'' cubes

Android Marshmallow

ZTE speed or Motorola moto g gen 2 phones

Multiple inputs from a controlling phone and various sensors

Output to multiple motors and servos

答:
Robot-Control-Systems are normally designed as Realtime-Systems. Traditionell programming concepts like loops or procedures will not work. Instead a time-based-schedule is necessary:

```<code>0 walkforward
1 walkforward
2 stop
3 turnright
4 walkforward
5 stop
</code>```

The first parameter is the timeindex, and the second parameter is the actual command. This plan has to be generated on the fly, according to the obstacles which can be changed during runtime. The scheduling-plan can be seen like a chessboard notation (1. a2a4 c2d2) but for the domain "robot control".

To create such realtime-plans one possibility is indead machinelearning. The commands of the human-operator will be recorded and set in relation to the current situation. And with a little bit of luck, this will work for new situations. Another (better) solution would be to see this as an np-hard problem like Sudoko or Travelling-salesman: for given situation and a given goal a number of actions has to be found.

The idea of break down a robot-control-system into different modules like lokalize, navigation, locomotion, manipulate and mission control is not bad, but it is only additionell. It is a way for generating the overall realtime-plan.

Body:
Robot-Control-Systems are normally designed as Realtime-Systems. Traditionell programming concepts like loops or procedures will not work. Instead a time-based-schedule is necessary:

0 walkforward
1 walkforward
2 stop
3 turnright
4 walkforward
5 stop

The first parameter is the timeindex, and the second parameter is the actual command. This plan has to be generated on the fly, according to the obstacles which can be changed during runtime. The scheduling-plan can be seen like a chessboard notation (1. a2a4 c2d2) but for the domain "robot control".

To create such realtime-plans one possibility is indead machinelearning. The commands of the human-operator will be recorded and set in relation to the current situation. And with a little bit of luck, this will work for new situations. Another (better) solution would be to see this as an np-hard problem like Sudoko or Travelling-salesman: for given situation and a given goal a number of actions has to be found.

The idea of break down a robot-control-system into different modules like lokalize, navigation, locomotion, manipulate and mission control is not bad, but it is only additionell. It is a way for generating the overall realtime-plan.


问:
Fetching encoder value with SimpleMotion library by Granite Devices
Is it possible to fetch the current encoder position of a drive with the old SimpleMotion library (not V2) and a Granite Devices VSD-E through the smRawCommand(...) function?

答:
Have you checked the SimpleMotion library files yet? You can get them from here.

Please note that the old version for SimpleMotion is not supported anymore.


问:
Issue with series command in MATLAB
I have a controller and a plant in series. The controller is 3 input 3 output MIMO system and the plant is also 3 input 3 output system. The bode of the open loop gain, i.e.,

$$D(z)=C(z)*G(z)$$

appears to be different when using

$$series(C(z),G(z))$$ versus

$$
D(z) = C_1(z)G_1(z) + C_2(z)G_2(z) + C_3(z)G_3(z) \\
$$

Theoretically, I believe both are same. However, the latter method gives a different bode with undamped peak, unlike the one using the series command.

The approach using
$
D(z) = C_1(z)G_1(z) + C_2(z)G_2(z) + C_3(z)G_3(z)
$
is most suitable, according to me, as it gives more clarity.

Can anyone share idea on why this misbehaviour exists?

答:
You haven't posted any specifics about your plant or the controller, and I'm not positive I understand your terminology.

You said it's a MIMO system, but then you give the combined block (controller/plant) as:

$$
D(z) = C_1(z)G_1(z) + C_2(z)G_2(z) + C_3(z)G_3(z) \\
$$

A couple of points here:

It looks to me like $D(z)$ is now a singular value and not a vector of outputs.

Ignoring the point above, it doesn't look like you have any method for interaction between inputs. The way you wrote the "long" model implies that $G_1$ can be separated from $G_2$ and $G_3$, which means that $G_1$ has no impact on the others and vice-versa. This would mean that you don't actually have a MIMO system, just a group SISO systems.

Per point (1) above, I would expect your $D(z)$ definition to look more like:

$$
D(z) = \left[ \begin{array}{}
C_1(z)G_1(z) \\
C_2(z)G_2(z) \\
C_3(z)G_3(z)
\end{array} \right] \\
$$

That gives you a vector of outputs, but again, if $C_x$ has no impact on any of the other $C$ states, and $G_x$ has no impact on any of the other $G$ states, then you're saying that there's no coupling between any of those states and thus you don't have a MIMO system; it's more of a diagonal or block diagonal system where each input/output pair is distinct and unrelated to any other pair.

Unfortunately, this is about as helpful or insightful as I can be without specifics. If you could edit your post to show what you're using for the controller and plant, some plots of what your outputs are looking like, etc., then maybe I could say something more, but at the moment this is the best answer I can give.

Title: Issue with series command in MATLAB


问:
How do I measure the distance that a cord (string) has moved?
For a pet project, I am trying to fly a kite using my computer.  I need to measure how far a cord extends from a device.  I also need to somehow read out the results on my computer. So I need to connect this to my pc, preferably using something standard like USB.

Since the budget is very small, it would be best if I could get it out of old home appliances or build it myself.

What technology do I need to make this measurement?

答:
I like @kramer65's answer but because you indicated you might like to make an encoder, I thought I'd share some information on that.

http://www.societyofrobots.com/sensors_encoder.shtml shows a very common way of making a rotary encoder. You attach a disk with evenly spaced holes or reflective surfaces depending on implementation to your shaft. You align a light sensor with the holes and count the number of holes that pass to determine how much the shaft has turned. This has the disadvantage of not being able to determine direction but there was ways to modify the design to add that feature.

More information can be found in these places:

http://groups.csail.mit.edu/mac/users/pmitros/encoder/

http://thedenneys.org/pub/robot/encoders/


问:
Issue with open loop gain computation

Is (G1(z)*C1(z))+(G2(z)*C2(z))+(G3(z)*C3(z))  the right way of computing open loop gain for the attached block diagram. The system order differs from the order achieved using series(G(z),C(z)). Could any one help?

答:
No, for reasons I explained in the other question you asked, that is not the correct way to calculate the open-loop gain.

At-a-glance I can tell you it's wrong because it doesn't take the correct form. It's like asking if 50 miles is too fast or if <13,20> gives you a 3D coordinate - your output form isn't of a type that satisfies the question.

You are asking for an open-loop gain, like it's a singular value, when you should be asking for the open-loop gains, because you need an open-loop gain between each input and each output.

I would suggest you look at relative gain arrays if this is what you're interested in. You haven't clarified what your equations are, what your controller is, what your plant is, what your application is, etc., so it's not really possible to give too much help beyond this.

I will point out that it looks like you're setting yourself up for conflicting or contradictory control by having position, speed, and torque be inputs. What if the user wants zero position and 100% speed? What if the user wants zero speed and 100% torque?

If I had to guess, I'd say your application is more like a position-controlled servo, where you want to limit torque and speed. You probably don't actually care about controlling torque and speed as long as they don't exceed specific thresholds. In that case, you would have one open-loop gain - the input position reference to the output position feedback.

But, that's all speculation. Until you clarify what you're trying to do, it's hard to give meaningful assistance.

Title: Issue with open loop gain computation


问:
Paradox: I can't use accelerometer measurements to obtain information about my states in a quadcopter?
I'm currently developing an EKF to estimate the position and orientation of a quadcopter. My state vector is comprised of 3D position, 3D velocity, 3 euler angles and the angular velocity vector.

Right now I'm looking into the measurement equation for the accelerometer.

If I understood correctly, an accelerometer measures "proper acceleration", instead of coordinate acceleration, that is, it measures the acceleration of the body w.r.t to a free-falling coordinate system.

If this is the case, and supposing the only forces acting on the body are the upward thrust given by the propellers, $\vec{T}$, and earth's gravitational force, $m\vec{g}$, then I understand that the only acceleration that would be measured by the accelerometer is the one caused by $\vec{T}$ (since the free-falling frame has no way of measuring the acceleration caused by $m\vec{g}$, because it is also being accelerated by it).

If this also the case, then I note that the vector $\vec{T}$, when expressed in the body coordinate frame (i.e. a coordinate frame fixed at the center of mass of the body, and always aligned with the body's orientation) does not depend on any of the states whatsoever. For example, if the propellers are assembled such that $\vec{T}$ is always perpendicular to the plane where the propellers are, then $\vec{T}$ in the body frame is specified as $(0,0,\alpha)^T$, where $\alpha$ is the magnitude of the thrust given.

Which leads me to conclude that (since the measured acceleration doesn't depend on the states) I can't use accelerometer measurements to obtain more information about any of my states (??). This conclusion seems paradoxical to me, and that's why I ask this here. Could someone please point the mistake in my reasoning, or elucidate why this is not a paradox?

答:
Accelerometers measure kinematic acceleration with the addition of gravity. So for an accel to measure 0, the vehicle would need to be accelerating downward at $g$. To get inertial acceleration out of an accel measurement one simply needs to subtract the acceleration measured by the IMU when the IMU is static.

So, assuming the coordinate system of the drone is in line with the inertial coordinate system

\begin{equation}
\ddot r = z_{acc} - g
\end{equation}

where $\ddot r$ is acceleration  of the drone in inertial space, $z_{accel}$ is the vector of accelerations measured by the IMU, and $g$ is the vector of accelerations experienced when the vehicle is not accelerating relative to the inertial frame.

Transformations need to get involved once the vehicle is not longer level but it's about the same.


问:
Having a hard time understanding this equation in monocular EKF SLAM
Reading this paper on visual odometry, where they have used a bearing vector to parameterize the features. I am having a hard time understanding what the state propagation equation for the bearing vector term means :

The vector N is not mentioned in the equations, so its not very clear what it does. Would really appreciate if someone would help me understand it :)

答:
It appears to me that the notation represents taking an arbitrary 3-vector, applying a rotation matrix to the 3-vector based on the bearing 3-vector, and then removing one of the dimensions of the result.  If the arbitrary 3-vector and the bearing 3-vector are the same, then the result of the rotation should be [ 1 0 0 ], and the first coordinate would be removed, since the projection of that vector onto the plane orthogonal to it is zero.  For an arbitrary 3-vector, you get the components of the projection onto a plane that is normal to the bearing 3-vector.

As to how to compute the rotation matrix based on the bearing 3-vector, I would defer to the following:

https://math.stackexchange.com/questions/114512/how-to-find-the-orthonormal-transformation-that-will-rotate-a-vector-to-the-x-ax


答:
If the drone is not falling (holding height in the sky), and it's not accelerating in any particular direction, then the accelerometer should be reading:

$$
a = \left[ \begin{array}{}
g_x \\
g_y \\
g_z
\end{array}\right]
$$

where $g_N$ is the component of gravity along each axis. If the drone is upright and stationary, and the accelerometer is oriented such that the z-axis points up, then the accelerometer would read:

$$
a = \left[ \begin{array}{}
0 \\
0 \\
g
\end{array}\right]
$$

However, if the drone is tilted and not accelerating, then the gravity vector will appear in portions along the axes not orthogonal to gravity.

The Madgwick filter (which works pretty awesome by the way) utilizes this to determine the orientation of the accelerometer. The accelerometer is normalized and the component of acceleration on each axis gives the orientation.

Given the orientation, subtract out gravity, you're left with the acceleration along each axis. Double integrate those accelerations and you get position.

There are of course issues if you're accelerating in a particular direction that isn't in-line with gravity, because normalizing the acceleration will cause the "gravity vector" to skew towards the axis of motion. However, this is generally overcome by including gyroscope measurements with the orientation estimate.

It's important to remember too that these are all estimates. Unless you have a position measurement or an orientation measurement device, you'll never actually get measurements; the best you can do is generate pose estimates because there's no way to correct any accumulated errors.

Gyroscope readings are generally very accurate as a short-term orientation estimate and very poor as a long-term orientation estimate because of gyroscope drift. The accelerometer's gravitational orientation result, because it is affected by the motion of the sensor, generally results in poor short-term orientation measurements but very accurate long-term measurements.

So, between the accelerometer and gyroscope, you should be able to get very good orientation estimates (did I mention the Madgwick filter is really good? Don't kill yourself reinventing the wheel; the filter is open-source!). Again, once you have the orientation, subtract the gravity vector and you're left with only the accelerations caused by motion of the device.

Accelerometers suffer from drift too, though, so you're not going to get any high-precision position estimates from an accelerometer over any significant period of time.

Title: Paradox: I can't use accelerometer measurements to obtain information about my states in a quadcopter?

Body:
I'm currently developing an EKF to estimate the position and orientation of a quadcopter. My state vector is comprised of 3D position, 3D velocity, 3 euler angles and the angular velocity vector.

Right now I'm looking into the measurement equation for the accelerometer.

If I understood correctly, an accelerometer measures "proper acceleration", instead of coordinate acceleration, that is, it measures the acceleration of the body w.r.t to a free-falling coordinate system.

If this is the case, and supposing the only forces acting on the body are the upward thrust given by the propellers, $\vec{T}$, and earth's gravitational force, $m\vec{g}$, then I understand that the only acceleration that would be measured by the accelerometer is the one caused by $\vec{T}$ (since the free-falling frame has no way of measuring the acceleration caused by $m\vec{g}$, because it is also being accelerated by it).

If this also the case, then I note that the vector $\vec{T}$, when expressed in the body coordinate frame (i.e. a coordinate frame fixed at the center of mass of the body, and always aligned with the body's orientation) does not depend on any of the states whatsoever. For example, if the propellers are assembled such that $\vec{T}$ is always perpendicular to the plane where the propellers are, then $\vec{T}$ in the body frame is specified as $(0,0,\alpha)^T$, where $\alpha$ is the magnitude of the thrust given.

Which leads me to conclude that (since the measured acceleration doesn't depend on the states) I can't use accelerometer measurements to obtain more information about any of my states (??). This conclusion seems paradoxical to me, and that's why I ask this here. Could someone please point the mistake in my reasoning, or elucidate why this is not a paradox?


问:
What sensors and algorithms are used in Digital Pen for tracking hand writing?
Could anyone tell what are the sensors used in a digital pen which or specifically Equil smart pen and smart marker which can track hand writings. Is it MEMS based??If Yes, Is it MEMS accelerometer only or a combination of MEMS sensors like gyroscope and magnetometer and acceleometer? What algorithms are used here?  </p>

答:
Most smart pens use a number of differenst sensors different types of MEMS but more importanly they use optical sensors. Pen tip motions are recreated based on measurements of all sensors using sensor fusion.

Optical sensors (Ir cameras or visible cameras) and ultrasonic sensors are used for pose (position and orientation measurment). Some pens, as the Equil use an external sensing (with infrared and ultrasound sensors) unit which tracks the pen relative to the sensor. Other solution include small cameras near the tip of the pen which identify special markers on the paper and so can calculate the relative position of the pentip on the paper. These directly measure pen relative to the paper.

The optical pose sensing is more reliable then the MEMS sensors since they directly measure positions and so they do not suffer from drift. However, MEMS sensors help to recreate motions using sensor fusion. Both giros and acceleromaters offer usefull input to the sensor fusion process, using both of them if just one type may be an economic decision and not a technical one.

I am not sure to what extent magnetometers are/can be used since magnetic field vector fo the earth, which they can sense is dependent on geographical location. Also they are effected by nearby electro-magnetic fields (e.g. motors, maybe large LCD screens due to their inverter).


问:
Robots minimum distance
I am trying to implement a mechanism to make robots avoid being too close (Say in a distance less than d). I am not familiar with those systems and I have to implement a strategy to avoid robots being too close to each other. Could anyone recommend me some readings for such a problem or a set of keywords to search for? I don't know yet how to start.

答:
I'd  like to suggest that you start with a book or a course in principles of robotic motion for this topic.

Principles of Robotic Motion Provides a very detailed introduction to this topic starting chapter 3 onwards.

Alternatively, you might want to look for how to make efficient configuration spaces for robots to traverse.

Title: Robots minimum distance


问:
How to use registration inputs on a Kinetix servo?
I am working with a Kinetix 5500 servo drive with a rotational actuator to locate a keyway on a shaft. The shaft will rotate, and a laser sensor will detect the height difference when the keyway passes in front of the sensor. I need to record the position of the keyway so I can orient it to a known position.

The laser sensor will be wired to Registration Input 1 (see pages 62 and 65 of user manual above), to capture the position of the servo more accurately than using a sensor wired in to the PLC's I/O.

I have been able to find little information about how to actually use a registration input in my PLC program. I know there is some sequence that is required to arm the registration input to look for a transition, and then grab the saved servo position from the drive.

How does this process work? Is there documentation from Rockwell that I'm not finding? (I found a programming manual for PowerFlex, but not Kinetix)

Additional information: the PLC is an AB ControlLogix, and the drive is connected via Ethernet. I am familiar with setting up, tuning, and issuing Motion Direct commands for Kinetix drives.

Related nonexistent tags: Allen-Bradley, Kinetix, ControlLogix

答:
There are a few different ways that you can use this input.

One way is to use it in conjunction with homing.  Homing the servo is defining where position zero is located.  You initiate a "Home" by using the MAH (Motion Axis Home) instruction.  The Home function is configured in the Axis properties under the "Homing" tab.  You can get more information about Homing by pressing the Help button while on this tab, but here are a few pointers:

A Home can either be Active or Passive.  When an Active Home is performed, the servo will move until it finds the home position.  A Passive Home, on the other hand, does not initiate any motion; before or after initiating a Passive Home, you would need to initiate motion with another instruction.

The "Sequence" parameter defines how the drive knows it is in the home position.  In your case, you would choose "Switch," which tells the drive to use the "Home/Registration 1" input on the drive.  You select either "Normally Open" or "Normally Closed" to define what the state of the switch is when it is in the home position.

Another use for this input is in conjunction with the MAR (Motion Arm Registration) instruction.  This is done when you want to control the position of a continually rotating axis relative to something else.  When registration is armed, the drive watches for the next transition of the registration input and then logs the position at that point in the .Registration1Position field in the Axis Tag structure.  You can use this value to speed up or slow down the axis to maintain the relative position you are trying to achieve.  The configuration for this is done right on the MAR instruction; you select which input number to use (1=Registration Input 1), and what type of Trigger Condition you want to look for (0 = trigger on positive transition, 1 = trigger on negative transition).

You can find help for these instructions in the help files inside RSLogix5000, or you can read the Logix5000 Controllers Motion Instructions (motion-rm002) publication.


问:
Addressing the sample impoverishment in particle filter
I have implemented a particle filter algorithm for the state estimation of a mobile robot.

There are several external range sensors(transmitters) in the environment which gives information on the distance (radius) of the robot based on the time taken for the receiver on the robot to send back its acknowledgement. So, using three or more such transmitters it will be possible to triangulate the position of the robot.

The particle filter is initialized with 15000 particles and the sensor noise is relatively low (0.02m).

Update Phase: At each iteration a range information from an external sensor is received. This assigns higher weights to the particles along the radius of the external sensor. Not all the particles are equally weighted since the process noise is low. Hence in most of the cases, the particle relatively closer to the robot gets lower weight than an incorrect one that happens to be along the radius. The weight is a pdf.

Resampling Phase: At this stage, the lower weighted particle(the correct one) that has negligible weight gets lost because the higher weighted particle gets picked up.

All this happens at the first iteration and so when the range information from another sensor arrives, the robot is already kidnapped.

Googling around, said that this problem is called as sample impoverishment and the most common approach is to resample only when the particle variance is low. (Effective Sample Size < number of particles / 2)

But, when the particles are assigned negligible weights and there are relatively very few particles with higher weights, the diversity of the particles are lost at resampling phase. So, when the variance is higher resampling is done which removes the lower weighted particle and hence the diversity of the particles is lost. Isnt this completely the opposite of the above idea of ESS?

Is my understanding of sample impoverishment correct? Is there a way this issue can be fixed?

Any pointers or help would be highly appreciated.

答:
Your description of sample impoverishment and the way to fix it seems about right. Resampling only when the variance gets low is doing exactly what you are asking for when you say the measurements come in asynchronously. You can also improve matters by selecting the right resampling strategy. Using e.g. stratified resampling you can make sure that your higher than average weight particles won't get eliminated by chance. And yes, after resampling the particles are less diverse. That is the nature of the resampling process. Since you eliminate some particles, and copy others, the diversity after resampling is lower. You "smear" out e.g. increase diversity during the updating steps.

This can of course still lead to situations, where the bulk of your particles get deleted, and only a single particle is left (multiple times over) after resampling, because the weight before was so high. In this case its more likely that your update and measurement models don't fit very well.

Particle filters work very well with non-linearities in the regions where the probability density function of the prior is high. They don't do so well in the other regions and need tricks to make them work there.


问:
Illegal position data with Mitsubishi Melfa RV-2F-Q robotic arm
I have a project where I have to move a Mitsubishi Melfa RV-2F-Q robot to a position/orientation from an external source, so there are no pre-defined points available.

The problem I keep running into is that even if I give it a reachable position (within operating range), or even a position that is almost the same as its current one, it fails to move there with the error:

L2802 Illegal position data (dstn)

What causes this and how do I avoid it?

答:
Robots use many coordinate system and for each position you transmit to the robot, it has to be specified in which coordinate system is it defined. If you do not define it explicitly, an implicit definition is used, which may not correspont to the one in which the current postition is defined. So what you think is exaclty near the robot, may be actually out of the operating range.

A simple case is when you give the robots coordinates in its base frame, but it expects realtive coordinates (in tool frame). If the current pose in the base frame is [1, 0, 0, 0, 0, 0] any you try to reach the coordinates [1.1, 0, 0, 0, 0, 0], in realtive coordinates you should transmit [0.1, 0, 0, 0, 0, 0]. The coordinates [ 1.1, 0, 0, 0, 0, 0] realative to the tool frame are be most probably out of the workspace. (vice versa is also true)

Also check the allowed operation envelope. An incorrect setup of the operation envelope could also cause similar problems.


问:
Does simulink accepts robotics toolbox in matlab?
I'm trying to make a model for a robotic arm on simulink and then excute it on arduino controller but i want also to use robotics toolbox to calculate forward and inverse kinematics so i need to add matlab function in my simulink model i tried it but it keeps giving errors but when i try it away from simulink it gives rsults ! is that because robotics toolbox works only with matlab but not with simulink?

the code in matlab function

```<code> function [theta1, theta2, theta3, theta4] = fcn(x, z, y)

L(1)=Link([0,0.03,0,pi/2,0]);
L(2)=Link([0,0,0.12,0,0]);
L(3)=Link([0,0,0.1,-pi/2,0]);
L(4)=Link([0,0,0,0,0]);
robot = SerialLink(L,'name','robot');

robot.tool=transl(0,0,.08);
robot.base=transl(0,0,.07);

    T=transl(x,y,z);
    q=robot.ikine(T,[0,0,0,0], [1 1 1 0 0 1]);
    q(1)=fix(q(1)*(180/pi));
   q(2)=fix(q(2)*(180/pi));
    q(3)=fix(q(3)*(180/pi));
    q(4)=fix(q(4)*(180/pi));

          theta1=q(1)
    theta2=q(2)
    theta3=q(3)
    theta4=q(4)
</code>```

but when i try it in command window away from simulink it gives results for x=1 y=1 z=1 and higher values

```<code>   Warning: solution diverging at step 245, try reducing alpha
 > In SerialLink/ikine (line 260)
  Warning: solution diverging at step 459, try reducing alpha
 > In SerialLink/ikine (line 260)
 Warning: solution diverging at step 533, try reducing alpha
 > In SerialLink/ikine (line 260)
 Warning: ikine: iteration limit 1000 exceeded (row 1), final err 1.790820
 > In SerialLink/ikine (line 179)

 theta1 =

  -21996

theta2 =

-1050694

theta3 =

 2711696

theta4 =

 2848957
</code>```

but for x=0.2  y=0.2 z=0.2 it gives that error

```<code>      .
      .
      .
      .
> In SerialLink/ikine (line 260)
Warning: solution diverging at step 567, try reducing alpha
> In SerialLink/ikine (line 260)
Warning: solution diverging at step 570, try reducing alpha
> In SerialLink/ikine (line 260)
Warning: solution diverging at step 574, try reducing alpha
> In SerialLink/ikine (line 260)
Warning: solution diverging at step 578, try reducing alpha
> In SerialLink/ikine (line 260)
Error using tr2angvec (line 97)
matrix not orthonormal rotation matrix

Error in SerialLink/ikine (line 191)
        [th,n] = tr2angvec(Rq'*t2r(T));
</code>```

答:
You appear to be using a third-party toolbox, the Robotics Toolbox. If it's not running in Simulink, then it might not run in Simulink. If you want something that does, try the official toolbox. Mathworks generally offers trials of all their toolboxes.

Your first error, "Simulink does not have enough information to determine the output size of this block," can typically be solved by initializing the variables before assignment and/or by setting their size in the properties menu for the function block (as opposed to leaving it set as "inherited").

That said, I'd like to point out that your results are not correct and you are get a lot of bad warnings that you seem to be ignoring when you show that it "works" when you're not using Simulink.

Diverging solutions mean something is wrong, iteration limits being exceeded mean something is wrong, and did you even look at the numbers you were given?

What does it mean to you when the output you're getting is something like theta4 = 2848957?? Does 2.8 million radians (or even degrees) seem like a quality answer?

If you really need to use this toolbox with Simulink then at least make sure you're using it correctly and getting correct answers first before you try to port to Simulink.

ContentLicense: CC BY-SA 4.0


答:
Classical odometry sensors:

IMU:gyro, accelerometer, compass (Wiimote IMU - not infrared)

Optical flow (optic mouse)

encoders (like mouse marble)

These systems are cheap and easy to apply. However these sensors needs sensor fusion and filtering because of error cumulation. These sensors are not suitable for long processes.

Reference based systems:

sensor surfaces (touchscreen, electromagnetic/resistive/capacitve etc.)

Infrared reference tracking (wiimote IR and white board)

Multi point reference: (ultrasonic transmitter and receivers) source emits ultrasonic waves and triliteration/triangulaion made accoridng to 3 or more receivers

Optical tracking (VICON, multiple cameras placed allover the target/pen and according trace of pointers which placed on target motions are modelled )

these systems are more accurate but needs computational power and a lot of hardware. Again, you should use sensor fusion and filtering. You can also search triangulation and trileteration methods and algorithms.

For sensor fusion and filtering algorithms please search:

Kalman filter/ Extended Kalman Filter

Complementary Filter

Particle Filer

Title: What sensors and algorithms are used in Digital Pen for tracking hand writing?

Body: Could anyone tell what are the sensors used in a digital pen which or specifically Equil smart pen and smart marker which can track hand writings. Is it MEMS based??If Yes, Is it MEMS accelerometer only or a combination of MEMS sensors like gyroscope and magnetometer and acceleometer? What algorithms are used here?  </p>


问:
Intelligent robotic arm and its control system
As I know, most of robotic arms are specific-purpose and usually work under supervision of an expert, such as surgery robot. So, is this relevant for a robotic arm to be intelligent and autonomous? If so, how its control system may be different from non-intelligent ones?</p>

答: I believe you are only noticing the robotic systems that get a lot of press coverage.   Specialized, supervisory-controlled robotic systems are definitely not the most common.  Check out this list of industrial robotic systems for quantities of general-purpose, reprogrammable, robot systems for comparison purposes.   Whether or not reprogrammable industrial robots are intelligent is a matter of speculation and argumentation.   It depends on your definitions of autonomous and intelligent.


答: At first it is important to distinguish between a Robot Control System itself and the development process to program such a system. A reasonable Control System is NREC Operator Control System which can control a robot arm. But a way more interesting is to program such systems. In most cases at the beginning is a teleoperating system, where a human-operator controls a robot with a Xbox-Controller. Than a parser is programmed for detecting actions. The next step converts the parser into a trajectory generator. Then, a time-scheduler follows and perhaps a sampling-based solver like DARRT.</p>

Title: Intelligent robotic arm and its control system

Body: As I know, most of robotic arms are specific-purpose and usually work under supervision of an expert, such as surgery robot. So, is this relevant for a robotic arm to be intelligent and autonomous? If so, how its control system may be different from non-intelligent ones?</p>


问:
How can I compare robot and human hands?
I want a comparison between human hands and robot hands, with respect to grasping and squeezing objects.

How can I perform such a comparison, using sensors, as one would with a benchmarking database? Is there a standard, or ready-to-use, system?

答: The only benchmark I know in development is at NIST.  Their Intelligent Systems Division project page should help.  On that page they discuss the specific possible metrics of grasp cycle time, grasp efficiency, finger strength, grasp strength, in-hand manipulation, object pose estimation, slip resistance, split cylinder artifact, touch sensitivity, finger force tracking, and sensor calibration.</p>


问:
Kalman Filter GPS + IMU
I know this probably has been asked a thousand times but I'm trying to integrate a GPS + Imu (which has a gyro, acc, and magnetometer) with an Extended kalman filter to get a better localization in my next step. I'm using a global frame of localization, mainly Latitude and Longitude. I kinda 'get' the kalman equations but I'm struggling in what should be my actual state and what should be my sensor prediction.

I have in one hand the latitude and longitude of my GPS and in euler degrees the roll, pitch and yaw of my IMU (which is already fused by some algorithm on board the chip I think) in euler degrees. I think I can throw the pitch and roll away.

And I know I have to show a function for my state $$ x_t =  f (x_{t-1}, \mu_{t-1}) $$ and a function that predicts what my sensor is seing at the step $t$
$$ \mu_{t} = h(x_t)$$

The thing is I dont know on what these functions should depend on, should my state be about the gps? In that case how can I predcit the next yaw read since I don't think I can get the rotation from a difference from gps location.

On the other side if my state is the yaw, I need some kind of speed, which the GPS is giving me, in that case would kalman work? Since I'm using the speed from the GPS to predict the next GPS location.

Long story short I dont know what my state and sensor prediction should be in this case.

Thanks In advance.

Edit: I have an ackerman steering mobile robot with no encoders which has mounted a GPS and and IMU (gyr acc and mag). The imu fuses thes values into euler degrees and the GPS gives me lat and longitude.

答: id recommend estimating position, velocity, and bias in the yaw reading from the imu. You'd assume the yaw bias is a first order system driven by zero mean Gaussian noise. You can also estimate biases in your accel readings if you expect significant drift during operation. </p>

Title: Kalman Filter GPS + IMU


问:
How to obtain stereo correspondences and what exactly is a disparity map?
I am currently reading into the topic of stereo vision, using the book of Hartley&Zimmerman alongside some papers, as I am trying to develop an algorithm capable of creating elevation maps from two images.

I am trying to come up with the basic steps for such an algorithm. This is what I think I have to do:

If I have two images I somehow have to find the fundamental matrix, F, in order to find the actual elevation values at all points from triangulation later on. If the cameras are calibrated this is straightforward if not it is slightly more complex (plenty of methods for this can be found in H&Z).

It is necessary to know F in order to obtain the epipolar lines. These are lines that are used in order to find image point x in the first image back in the second image.

Now comes the part were it gets a bit confusing for me: Now I would start taking a image point x_i in the first picture and try to find the corresponding point x_i’ in the second picture, using some matching algorithm. Using triangulation it is now possible to compute the real world point X and from that it’s elevation. This process will be repeated for every pixel in the right image.

In the perfect world (no noise etc) triangulation will be done based on

```<code>x1=P1X
x2=P2X
</code>```

In the real world it is necessary to find a best fit instead.

Doing this for all pixels will lead to the complete elevation map as desired, some pixels will however be impossible to match and therefore can't be triangulated.

What confuses me most is that I have the feeling that Hartley&Zimmerman skip the entire discussion on how to obtain your point correspondences (matching?) and that the papers I read in addition to the book talk a lot about disparity maps which aren’t mentioned in H&Z at all. However I think I understood correctly that the disparity is simply the difference x1_i- x2_i?

Is this approach correct, and if not where did I make mistakes?

答:
How to obtain your point correspondences

There are many ways to do it, which can basically be classified into two categories: feature-based and dense matching.

Feature-based methods include using corner detectors, SIFT/SURF/ORB descriptors and other similar feature detectors that provide point to point correspondences. A few of those methods that are implemented in OpenCV are compared here.

Dense-matching methods usually involve some kind of comparison between sliding windows in both images. The most used approach is SSD or variants. A few of these variants are listed here.

What are disparity maps

Your intuition is right: disparity, in a rectified image, is defined as the difference between the x coordinate of a point in two images. It had a neat relationship from projective geometry: disparity = 1/depth. This stackoverflow answer has a few pointers about it. Disparity maps are basically images where each pixel value is the disparity of that point, so it gives you a sense of depth - the further away from the camera, the darker.

EDIT: Yes, in general terms your approach is correct AFAIK. Your description lacks detail, but as you mentioned you can find out how to do each of these pieces in H&Z.


答:
This is a complete re-working of the answer I had originally provided. If you're curious, you can check the edit history and see what was posted earlier.

In comments to this question, OP stated that they might be able to get throttle and steering angles for the robot, but they probably wouldn't be accurate. That's okay; it's better than nothing.

OP also stated that the IMU outputs a fused orientation, where the fused orientation is from the accelerometer, gyro, and magnetometer.

The accelerometer output of the IMU may or may not be corrected by the orientation. If it is, great. If not, it's a straightforward conversion provided you pick the correct form; I think generally what you would want is the XYZ Tait-Bryan rotation matrix, but definitely check with the manufacturer and absolutely run through copious amounts of test data to check the results yourself.

Side note here, I build quick visualizers for myself all the time and highly recommend you do it for yourself, too. Here's a quick Matlab snippet to use, assuming you have a variable time that has all of your time indices. If you just have the sample intervals, you can make time as the cumulative sum of the sample intervals; time = cumsum(sampleIntervals);.

width = 1;
length = 2;
height = 0.5;
shapeTop = [...
    -width/2, -length/2; ...
    -width/2, length/2; ...
    width/2, length/2; ...
    width/2, -length/2];
shapeTop(:,3) = ones(4,1)*height/2;
shapeBottom = shapeTop;
shapeBottom(:,3) = shapeBottom(:,3) - ones(4,1)*height;
shapeVertices = [shapeTop;shapeBottom];
shapeFaces = [...
    1,2,3,4,1; ...
    5,6,7,8,5; ...
    8,5,1,4,8; ...
    7,6,2,3,7; ...
    5,6,2,1,5; ...
    8,7,3,4,8];
shapeColor = [0.6 0.6 1]; % Light blue

figure(1)
shapePatch = patch(...
    'Faces', shapeFaces, ...
    'Vertices', shapeVertices, ...
    'FaceColor', shapeColor);
axis equal;
xlabel('X-Axis')
ylabel('Y-Axis')
zlabel('Z-Axis')
view([60,20]);

tic
while true
    elapsedTime = toc;
    if elapsedTime > time(end)
        break;
    end
    currentSample = find(time>=elapsedTime,1);
    rotMatrix = EulerToRotationMatrix(...
        EulerX(currentSample) , ...
        EulerY(currentSample) , ...
        EulerZ(currentSample));
    tempVertices = shapeVertices;
    tempVertices = (rotMatrix*tempVertices.').';
    set(shapePatch,'Vertices',tempVertices);
    drawnow;
    pause(0.01);
end
```

This code runs through all of your sample data, at a 1:1 playback speed, and updates a shape's orientation according to your X/Y/Z angles. EulerToRotationMatrix is a function you make yourself that accepts your Euler x/y/z angles and returns the appropriate rotation matrix. That's the part you need to play around with, but again I'm pretty sure you want Tait-Bryan XYZ.

Record multiple test data sets of you moving your IMU from one starting pose to another starting pose, and take different paths to end at the same orientation. When you watch the playback you'll see pretty much immediately if you're doing the rotation matrix stuff correctly.

So anyways, once you're sure you've got your rotation matrix, if you need to correct the accelerometer data, it's just accNew = rotMatrix*accOld;.

Now, there are four possible ways to update yaw information:

From a model using inputs (throttle and steering angle).

Magnetometer for angle relative to magnetic North.

Gyro readings from IMU

Heading required to move in a straight line from the previous GPS fix to the current GPS fix.

The GPS location is a Gaussian distribution about your actual location, so I would be very reluctant to use option 4 above for anything.

The IMU already fuses the magnetometer and gyro data into one yaw reading, so then the only thing that you can do to improve your yaw estimate is with the model. If you can't get access to the inputs (throttle/steering angle) then you're stuck with the yaw angle the IMU gives you.

If you do get access to the inputs, then you can model the x/y/$\theta$ rates as follows:

$$
\dot{x} = v\cos(\frac{\pi}{2} - \theta) \\
\dot{y} = v\sin(\frac{\pi}{2} - \theta) \\
\dot{\theta} = \frac{v}{L}\tan(\delta) \\
$$

where $\theta$ is the heading, measured positive CW from the +y-axis; $\delta$ is the steering angle, measured positive CW from the forward vehicle direction, $L$ is the wheel base, the distance between the front and rear axles, and $v$ is the speed of the vehicle.

The model of how your vehicle parameters (position/heading) update are nonlinear to the point that I wouldn't try to put it in state space form. You really do need the actual $\cos$ and $\sin$ functions; the small-angle approximations wouldn't really work here. That's okay! You're using the extended Kalman filter, so you don't need to try to linearize the model.

I think I'd probably try to model the throttle signal as a first-order speed regulator, such that:

$$
\dot{v} = \frac{c\left(\mbox{throttle}\right) - v}{\tau} \\
$$

where $\tau$ is the time constant and $c$ is a value that scales the throttle to a speed. Start a test with the vehicle stationary, then give a step command for throttle. The long-term, stable speed can be used to determine $c$. Then you can figure, given a particular speed "request" (throttle position), what the time constant was.

The relationship between step input and system response is well known; 63.2% of final value at one time constant, 95.0% at three time constants, etc. I'd run multiple tests at each of a variety of throttle inputs and average the results.

Now, your more complete model for the system would be:

$$
\dot{v} = \frac{c\left(\mbox{throttle}\right) - v}{\tau} \\
v = v + \dot{v}\Delta T \\
$$
$$
\dot{x} = v\cos(\frac{\pi}{2} - \theta) \\
\dot{y} = v\sin(\frac{\pi}{2} - \theta) \\
\dot{\theta} = \frac{v}{L}\tan(\delta) \\
$$
$$
x = x + \dot{x}dT \\
y = y + \dot{y}dT \\
\theta = \theta + \dot{\theta}dT \\
$$

The generic Kalman model.

You're using the extended Kalman filter which, unlike the regular ("classic"?) Kalman filter, doesn't require a linear system. This is great because the system model is right above. Your states are position, speed, and yaw angle.

So you do your predict steps:

Predict the state estimate:

$$
\hat{x}_{t|t-1} = f\left(\hat{x}_{t-1} , u_{t-1}\right) \\
$$

Here $\hat{x}_{t|t-1}$ is your prediction of the state vector $x$, which is what the hat $\hat{ }$ means, for the current time step given the state vector at the previous time step, which is what the ${t|t-1}$ means.

Predict the error covariance estimate:

$$
P_{t|t-1} = F_k P_{t-1} F_k^T + Q_k \\
$$

Here $F_k$ is the Jacobian $\left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{t-1},u_{t-1}}$, $P$ is the error covariance (how much you "trust" the state estimate; smaller = more faith), $Q_k$ is the process noise matrix, which is typically a diagonal matrix of [numbers]. How do you measure process noise? There's a bunch of papers on the subject and you should check them out. Try putting 0.1 on the diagonal and see how that works. Generally the process noise is how you tune the filter. Again, the $t$ and $t|t-1$ mean the same as they did (current and current given the past).

Then you do the update steps:

Here's the tricky-ish thing for you. You have multiple sensors - a GPS and an IMU. What to do? If you had independent sensors, the answer would be update, then update again. For example, if you were using the IMU only for speed and yaw angle, and you were using the GPS only for position, then you would have the following residuals/innovations:

$$
\tilde{y}_{\mbox{IMU}} = \left[\begin{matrix}{}
\mbox{speed measurement} \\
\mbox{yaw angle measurement}
\end{matrix} \right] - \left[\begin{matrix}{}
0 & 1 & 0 \\
0 & 0 & 1
\end{matrix} \right] \left[\begin{matrix}{}
\mbox{position state prediction} \\
\mbox{speed state prediction} \\
\mbox{yaw angle state prediction}
\end{matrix} \right]
$$
$$
\tilde{y}_{\mbox{GPS}} = \left[\begin{matrix}{}
\mbox{position measurement}
\end{matrix} \right] - \left[\begin{matrix}{}
1 & 0 & 0
\end{matrix} \right] \left[\begin{matrix}{}
\mbox{position state prediction} \\
\mbox{speed state prediction} \\
\mbox{yaw angle state prediction}
\end{matrix} \right]
$$

You can then go on calculating the residual covariances and Kalman gains without worry because those measurements are independent and don't "overwrite" each other.

You're not so lucky; you have to deal with the fact that you've got two redundant measurements - IMU position and GPS position. The common way to handle the redundancy is to average the sensors before you send them to the filter. This works fine for two identical sensors, but that's not what you have.

A better way might be to weight the measurements by their covariances such that more "trustworthy" sensors get a bigger weight.

All that said, the two don't have to conflict; the IMU technically outputs an acceleration. If you just integrate one time to get speed and leave it at that, then suddenly the IMU and the GPS don't conflict and you can leave their measurements separate and not have to worry about conflict. This is the route I would take. If you find it unsatisfactory you can try integrating the IMU output again and blending it with the GPS data, but I don't think that's going to make things any better.

Hopefully I've made things much more clear.

A final note, you'll probably find also that your GPS and IMU don't update at the same rate. What to do then? There's a lot of papers about that, but they basically boil down to downsampling the higher frequency data or faking the low frequency data. Downsampling can be done dumb (discarding data) or smart (filtering data), just like faking the data can be done dumb (repeating the same value) or smart (some manner of extrapolating a projected sensor reading).

Title: Kalman Filter GPS + IMU

Body:
I know this probably has been asked a thousand times but I'm trying to integrate a GPS + Imu (which has a gyro, acc, and magnetometer) with an Extended kalman filter to get a better localization in my next step. I'm using a global frame of localization, mainly Latitude and Longitude. I kinda 'get' the kalman equations but I'm struggling in what should be my actual state and what should be my sensor prediction.

I have in one hand the latitude and longitude of my GPS and in euler degrees the roll, pitch and yaw of my IMU (which is already fused by some algorithm on board the chip I think) in euler degrees. I think I can throw the pitch and roll away.

And I know I have to show a function for my state $$ x_t =  f (x_{t-1}, \mu_{t-1}) $$ and a function that predicts what my sensor is seing at the step $t$
$$ \mu_{t} = h(x_t)$$

The thing is I dont know on what these functions should depend on, should my state be about the gps? In that case how can I predcit the next yaw read since I don't think I can get the rotation from a difference from gps location.

On the other side if my state is the yaw, I need some kind of speed, which the GPS is giving me, in that case would kalman work? Since I'm using the speed from the GPS to predict the next GPS location.

Long story short I dont know what my state and sensor prediction should be in this case.

Thanks In advance.

Edit: I have an ackerman steering mobile robot with no encoders which has mounted a GPS and and IMU (gyr acc and mag). The imu fuses thes values into euler degrees and the GPS gives me lat and longitude.

Body:
This is a complete re-working of the answer I had originally provided. If you're curious, you can check the edit history and see what was posted earlier.

In comments to this question, OP stated that they might be able to get throttle and steering angles for the robot, but they probably wouldn't be accurate. That's okay; it's better than nothing.

OP also stated that the IMU outputs a fused orientation, where the fused orientation is from the accelerometer, gyro, and magnetometer.

The accelerometer output of the IMU may or may not be corrected by the orientation. If it is, great. If not, it's a straightforward conversion provided you pick the correct form; I think generally what you would want is the XYZ Tait-Bryan rotation matrix, but definitely check with the manufacturer and absolutely run through copious amounts of test data to check the results yourself.

Side note here, I build quick visualizers for myself all the time and highly recommend you do it for yourself, too. Here's a quick Matlab snippet to use, assuming you have a variable time that has all of your time indices. If you just have the sample intervals, you can make time as the cumulative sum of the sample intervals; time = cumsum(sampleIntervals);.

width = 1;
length = 2;
height = 0.5;
shapeTop = [...
    -width/2, -length/2; ...
    -width/2, length/2; ...
    width/2, length/2; ...
    width/2, -length/2];
shapeTop(:,3) = ones(4,1)*height/2;
shapeBottom = shapeTop;
shapeBottom(:,3) = shapeBottom(:,3) - ones(4,1)*height;
shapeVertices = [shapeTop;shapeBottom];
shapeFaces = [...
    1,2,3,4,1; ...
    5,6,7,8,5; ...
    8,5,1,4,8; ...
    7,6,2,3,7; ...
    5,6,2,1,5; ...
    8,7,3,4,8];
shapeColor = [0.6 0.6 1]; % Light blue

figure(1)
shapePatch = patch(...
    'Faces', shapeFaces, ...
    'Vertices', shapeVertices, ...
    'FaceColor', shapeColor);
axis equal;
xlabel('X-Axis')
ylabel('Y-Axis')
zlabel('Z-Axis')
view([60,20]);

tic
while true
    elapsedTime = toc;
    if elapsedTime > time(end)
        break;
    end
    currentSample = find(time>=elapsedTime,1);
    rotMatrix = EulerToRotationMatrix(...
        EulerX(currentSample) , ...
        EulerY(currentSample) , ...
        EulerZ(currentSample));
    tempVertices = shapeVertices;
    tempVertices = (rotMatrix*tempVertices.').';
    set(shapePatch,'Vertices',tempVertices);
    drawnow;
    pause(0.01);
end

This code runs through all of your sample data, at a 1:1 playback speed, and updates a shape's orientation according to your X/Y/Z angles. EulerToRotationMatrix is a function you make yourself that accepts your Euler x/y/z angles and returns the appropriate rotation matrix. That's the part you need to play around with, but again I'm pretty sure you want Tait-Bryan XYZ.

Record multiple test data sets of you moving your IMU from one starting pose to another starting pose, and take different paths to end at the same orientation. When you watch the playback you'll see pretty much immediately if you're doing the rotation matrix stuff correctly.

So anyways, once you're sure you've got your rotation matrix, if you need to correct the accelerometer data, it's just accNew = rotMatrix*accOld;.

Now, there are four possible ways to update yaw information:

From a model using inputs (throttle and steering angle).

Magnetometer for angle relative to magnetic North.

Gyro readings from IMU

Heading required to move in a straight line from the previous GPS fix to the current GPS fix.

The GPS location is a Gaussian distribution about your actual location, so I would be very reluctant to use option 4 above for anything.

The IMU already fuses the magnetometer and gyro data into one yaw reading, so then the only thing that you can do to improve your yaw estimate is with the model. If you can't get access to the inputs (throttle/steering angle) then you're stuck with the yaw angle the IMU gives you.

If you do get access to the inputs, then you can model the x/y/$\theta$ rates as follows:

$$
\dot{x} = v\cos(\frac{\pi}{2} - \theta) \\
\dot{y} = v\sin(\frac{\pi}{2} - \theta) \\
\dot{\theta} = \frac{v}{L}\tan(\delta) \\
$$

where $\theta$ is the heading, measured positive CW from the +y-axis; $\delta$ is the steering angle, measured positive CW from the forward vehicle direction, $L$ is the wheel base, the distance between the front and rear axles, and $v$ is the speed of the vehicle.

The model of how your vehicle parameters (position/heading) update are nonlinear to the point that I wouldn't try to put it in state space form. You really do need the actual $\cos$ and $\sin$ functions; the small-angle approximations wouldn't really work here. That's okay! You're using the extended Kalman filter, so you don't need to try to linearize the model.

I think I'd probably try to model the throttle signal as a first-order speed regulator, such that:

$$
\dot{v} = \frac{c\left(\mbox{throttle}\right) - v}{\tau} \\
$$

where $\tau$ is the time constant and $c$ is a value that scales the throttle to a speed. Start a test with the vehicle stationary, then give a step command for throttle. The long-term, stable speed can be used to determine $c$. Then you can figure, given a particular speed "request" (throttle position), what the time constant was.

The relationship between step input and system response is well known; 63.2% of final value at one time constant, 95.0% at three time constants, etc. I'd run multiple tests at each of a variety of throttle inputs and average the results.

Now, your more complete model for the system would be:

$$
\dot{v} = \frac{c\left(\mbox{throttle}\right) - v}{\tau} \\
v = v + \dot{v}\Delta T \\
$$
$$
\dot{x} = v\cos(\frac{\pi}{2} - \theta) \\
\dot{y} = v\sin(\frac{\pi}{2} - \theta) \\
\dot{\theta} = \frac{v}{L}\tan(\delta) \\
$$
$$
x = x + \dot{x}dT \\
y = y + \dot{y}dT \\
\theta = \theta + \dot{\theta}dT \\
$$

The generic Kalman model.

You're using the extended Kalman filter which, unlike the regular ("classic"?) Kalman filter, doesn't require a linear system. This is great because the system model is right above. Your states are position, speed, and yaw angle.

So you do your predict steps:

Predict the state estimate:

$$
\hat{x}_{t|t-1} = f\left(\hat{x}_{t-1} , u_{t-1}\right) \\
$$

Here $\hat{x}_{t|t-1}$ is your prediction of the state vector $x$, which is what the hat $\hat{ }$ means, for the current time step given the state vector at the previous time step, which is what the ${t|t-1}$ means.

Predict the error covariance estimate:

$$
P_{t|t-1} = F_k P_{t-1} F_k^T + Q_k \\
$$

Here $F_k$ is the Jacobian $\left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{t-1},u_{t-1}}$, $P$ is the error covariance (how much you "trust" the state estimate; smaller = more faith), $Q_k$ is the process noise matrix, which is typically a diagonal matrix of [numbers]. How do you measure process noise? There's a bunch of papers on the subject and you should check them out. Try putting 0.1 on the diagonal and see how that works. Generally the process noise is how you tune the filter. Again, the $t$ and $t|t-1$ mean the same as they did (current and current given the past).

Then you do the update steps:

Here's the tricky-ish thing for you. You have multiple sensors - a GPS and an IMU. What to do? If you had independent sensors, the answer would be update, then update again. For example, if you were using the IMU only for speed and yaw angle, and you were using the GPS only for position, then you would have the following residuals/innovations:

$$
\tilde{y}_{\mbox{IMU}} = \left[\begin{matrix}{}
\mbox{speed measurement} \\
\mbox{yaw angle measurement}
\end{matrix} \right] - \left[\begin{matrix}{}
0 & 1 & 0 \\
0 & 0 & 1
\end{matrix} \right] \left[\begin{matrix}{}
\mbox{position state prediction} \\
\mbox{speed state prediction} \\
\mbox{yaw angle state prediction}
\end{matrix} \right]
$$
$$
\tilde{y}_{\mbox{GPS}} = \left[\begin{matrix}{}
\mbox{position measurement}
\end{matrix} \right] - \left[\begin{matrix}{}
1 & 0 & 0
\end{matrix} \right] \left[\begin{matrix}{}
\mbox{position state prediction} \\
\mbox{speed state prediction} \\
\mbox{yaw angle state prediction}
\end{matrix} \right]
$$

You can then go on calculating the residual covariances and Kalman gains without worry because those measurements are independent and don't "overwrite" each other.

You're not so lucky; you have to deal with the fact that you've got two redundant measurements - IMU position and GPS position. The common way to handle the redundancy is to average the sensors before you send them to the filter. This works fine for two identical sensors, but that's not what you have.

A better way might be to weight the measurements by their covariances such that more "trustworthy" sensors get a bigger weight.

All that said, the two don't have to conflict; the IMU technically outputs an acceleration. If you just integrate one time to get speed and leave it at that, then suddenly the IMU and the GPS don't conflict and you can leave their measurements separate and not have to worry about conflict. This is the route I would take. If you find it unsatisfactory you can try integrating the IMU output again and blending it with the GPS data, but I don't think that's going to make things any better.

Hopefully I've made things much more clear.

A final note, you'll probably find also that your GPS and IMU don't update at the same rate. What to do then? There's a lot of papers about that, but they basically boil down to downsampling the higher frequency data or faking the low frequency data. Downsampling can be done dumb (discarding data) or smart (filtering data), just like faking the data can be done dumb (repeating the same value) or smart (some manner of extrapolating a projected sensor reading).


问:
Matlab Inverse Kinematics 6 DOF
I'm trying to write an inverse kinematics Matlab code for a 6 DOF robotic arm that has the following link parameters:

Twist angle (alpha): [-90, 0, 90, -90, 90, 0]

Link length (a): [0, 0.5, 0, 0, 0, 0]

Offset distance (d): [0, 0.25, 0, 1, 0, 0.5]

and Px, Py, Pz are the following [1,1,0]

I'm using the following equations for theta 1,2 and 3 values (closed form solution):

As seen in the equations theta 1 and 2 have 2 two roots (2 possible solutions) thus, the robot has eight groups of inverse kinematics solutions. How do I modify my code to select the ideal solution for theta ?

```<code>%Theta 1
     theta1 = (atan2(real(py),real(px)))-atan2(real(d2),real(sign1*sqrt(px^2+py^2-d2^2)));

 c1 = cos(theta1);
 s1 = sin(theta1);

 %Theta 2
 A = (c1*px)+(s1*py);
 B = (A^2+pz^2+a2^2-d4^2)/(2*a2);

 theta2 = (atan2(real(A),real(pz)))-atan2(real(B),real(sign2*sqrt(A^2+pz^2-B^2)));

 c2 = cos(theta2);
 s2 = sin(theta2);

 %Theta 3
 A1 = (c2*px)+(s2*py);
 theta3 = (atan2(real(A1-(a2*c2)),real(pz+(a2*s2)))) - theta2;

 c3 = cos(theta3);
 s3 = sin(theta3);
</code>```

答:
Ideal solution can be defined in many ways.

The simplest way to choose one is to compare which of the 8 solutions is closest to your current pose in joint space. This is usually a good idea if you are moving along a line (or similar defined trajectores).

In practics some robot manufactruers have solved this using the status and turn variables. These variables are additional inputs for the inverse kinematics problem and  specify exactly which inverse kinematics solution is needed.

You can also check is one of the poses has a collision with the environmnent to filter infeasable poses.

You can calculate which pose offers the larges dexterity index

Title: Matlab Inverse Kinematics 6 DOF


答:
You should check out:

The Yale-CMU-Berkeley (YCB) Object and Model set.  It is designed for facilitating benchmarking in robotic manipulation. The set consists of objects of daily life with different shapes, sizes, textures, weight and rigidity, as well as some widely used manipulation tests. The set is associated with a model database which provides mesh models and high-resolution RGB-D scans of the objects for easy incorporation into manipulation and planning software platforms.

The The Yale Human Grasping Data Set.  It is a dataset of human grasping movements in unstructured environments. More than 27 hours of video with grasp, object, and task data from two housekeepers and two machinists are available.

The GraspIt! simulator.  It is a physics based simulator that can accommodate arbitrary hand and robot designs. It has a number of grasp quality metrics, but I don't know if it has a simulated human hand.

Title: How can I compare robot and human hands?

Body:
I want a comparison between human hands and robot hands, with respect to grasping and squeezing objects.

How can I perform such a comparison, using sensors, as one would with a benchmarking database? Is there a standard, or ready-to-use, system?


答:
As said by Jacob, sample impoverishment is inherit to the Sampling-Importance-Resampling family of particle filters.

An alternative solution which does require some extra effort is to switch to a Markov Chain Monte Carlo (MCMC) particle filter. MCMC relies on constructing a Markov chain that has a stationary distribution which is equal to the distribution you want to sample from. Meaning that if you draw samples from this Markov chain long enough, as $n\rightarrow\infty$, you will sample $x_n\sim P$, where P is the distribution you want to sample from.

The construction of such a Markov chain is often done through the Metropolis-Hastings algorithm. Here, a proposal state $x_k^*$ is generated from the current state $x_k$, which is most often done by a Gaussian around the current state: $x_k^*\sim N(x_k,1)$. After this, the likelihood of both the initial state (seed) and proposal are calculated. Based on the likelihood ratio, the proposal is accepted with some probability. A simple introduction to MCMC and the M-H algorithm is given here.

In a particle filtering context, generating proposals around an initial seed means that the diversity in particles will never diminish, as long as the proposals are generated such that at least some of them are accepted.

EDIT: There do exist other types of SIR-like particle filters which have their own way to combat degeneracy/sample impoverishment. I suggest looking at Auxiliary Particle Filtering, where an auxiliary variable is introduced during sampling. This variable selects particles based on their goodness-of-fit to current observations. This helps to better model the tails of the density (which is the weak point with sample degeneracy).

Title: Addressing the sample impoverishment in particle filter

Body:
I have implemented a particle filter algorithm for the state estimation of a mobile robot.

There are several external range sensors(transmitters) in the environment which gives information on the distance (radius) of the robot based on the time taken for the receiver on the robot to send back its acknowledgement. So, using three or more such transmitters it will be possible to triangulate the position of the robot.

The particle filter is initialized with 15000 particles and the sensor noise is relatively low (0.02m).

Update Phase: At each iteration a range information from an external sensor is received. This assigns higher weights to the particles along the radius of the external sensor. Not all the particles are equally weighted since the process noise is low. Hence in most of the cases, the particle relatively closer to the robot gets lower weight than an incorrect one that happens to be along the radius. The weight is a pdf.

Resampling Phase: At this stage, the lower weighted particle(the correct one) that has negligible weight gets lost because the higher weighted particle gets picked up.

All this happens at the first iteration and so when the range information from another sensor arrives, the robot is already kidnapped.

Googling around, said that this problem is called as sample impoverishment and the most common approach is to resample only when the particle variance is low. (Effective Sample Size < number of particles / 2)

But, when the particles are assigned negligible weights and there are relatively very few particles with higher weights, the diversity of the particles are lost at resampling phase. So, when the variance is higher resampling is done which removes the lower weighted particle and hence the diversity of the particles is lost. Isnt this completely the opposite of the above idea of ESS?

Is my understanding of sample impoverishment correct? Is there a way this issue can be fixed?

Any pointers or help would be highly appreciated.


问:
Motor Controller Configuration
With a group of students we are building an exoskeleton for paraplegics. I am part of the Software & Control department and we are designing the motor controller configuration. We have several requirements:

Size is one of our main requirements. We want the Exoskeleton to be
as slim as possible, so this is true for all the components as well.
We want the motor controller to be as small as possible.

The motor controller has to work with a brushless DC motor

A setpoint will be sent from a microcontroller to the motor
controller Two or three absolute encoders need to be connected,
this depends on the design of the joint.
**We are either going to
use position or torque control
We need one encoder for the motor
angle, and one for the joint angle. We might implement Series
Elastic Actuation in our joint and then we want to be able to
measure the deflection in the spring and thus need two encoders
for that.

A continuous power output in the range of 700 - 1200 Watt

While exploring several off-the-shelf possibilities, we came accross the  Elmo Gold Twitter and the IOMI Pro. One of the problems of these boards, is the amount of absolute encoders that can be connected. Both the Elmo and the IOMI board can either have 1 or no absolute encoders connected.

We came op with a solution, so that we are able to connect multiple encoders. In our exoskeleton we are going to use an EtherCAT Master-Slave configuration. The High-Level control (e.g. state-machine) is on the EtherCAT Master and sends joint angle setpoints. Our idea is to use a EtherCAT Slave as a sort of second controller, which gets the joint setpoint and the joint encoder values, calculates the motor setpoint and sends this out to a certain off-the-shelf controller like the Elmo Gold Twitter or the IOMI Pro.

My question is the following: Is this even a good solution? And what are other solution to this problem? Are there even better alternatives for a motor controller? Might it be a better idea to build and program your own motor controller? (please bare in mind that we have limited experience in that area)

I thank you all in advance for your reply!

Cheers!

Nathan

答:
Is this even a good solution?

That depends on what you call good. I would certainly make sure that you can justify the role of each component in the system. Why do you need some many encoders if current exoskeletons do just fine with less?
In order to decide if your solution is good or not, you should have  clear list of requirements (e.g. required torques, velocities, resolution, price, etc.) including also functional requirements (elasticity of joints, etc.) If all requirments are satisfied, your solution is good.

For possible improvements, I would advise you to define each control loop you plan to have and make sure that you have the required amount of sensors and controllers to implement it. By required amount I mean no less then necesary but also no more then necesary. Each sensor you use should participate in (at lease one) a control loop and you have to be sure that the sensor is not measureing a parameter which can be easily calculated from a different sensor with the required precision. Also consider the latencys on the bus system you plan to imlement in your control loops and decide if they are acceptanble. The main idea would be to use as few components as possible, so you should be able to answer the question "Why can't the task of this component be done by an already existing one?" for each component in you schematic.

And what are other solution to this problem?

When you combine mechanics, electronics and software, there are probably an infinity of possible solutions to a problem. You can consider diffferent actuation types, differetn motor types, differen communication systems, etc.

Might it be a better idea to build and program your own motor controller?

If plan to use industrial communication protocols (such as EtherCAt, or ProfiNet) I would advise against doing your own controller If using i2c for example is an option and this is a controls assignment (not a systems assignment), I would advise you to definetly make your own controllers. It might not work perfectly (it might not even work) but the amount you learn by programming your own controller is worth far more then buying one.

Title: Motor Controller Configuration


问:
How to perform this reference system transformation?
I have two quaternions that indicate the initial orientation of a four wheel robot, each one in relative to one reference systems.

The robot's orientation given by a quaternion q is not the same in the two reference systems: For one reference system the quaternion q1 might refer to the robot looking at positive x while the same quaternion components q1 in the second reference system might refer to the robot looking at the negative x.

I have two matrices which indicate the position of the robot in time in its correspondent reference system.

I want to find the correspondent points of the first matrix in to the second reference system. Each matrix is built with a different sensor, so the results will be similar but not exactly the same.

I think I should find the transformation from the first reference system to the second and then apply it for each point of the first matrix. How can I find this transformation? The translation part I think is clear, but the rotation not at all.

Edit:

@WildCrustacean

The solution proposed does not solve the problem, I think that the reason is because each system represents the robot in a different way.

In the initial one (A) the robot moving forward with no rotation would increase in the X axis. In the goal referential system (B) the robot moving forward with no rotation would increase in the Z axis. (See figure for more details)

```<code>First system (A)
 ______
|\  T  \
| \_____\     z
|B |    | : y ^
 \ | R  |    \|
  \|____|     +--> x

Second system (B)
 ______
|\  T  \
| \_____\     x
|B |    | :   ^
 \ | R  |     |
  \|____|     +--> z
               \
                y

R: Right side
B: Back side
T: Top
</code>```

I think I have to apply an extra rotation to change the initial quaternion that belongs to the first system to be in accordance with the second system before applying the transformation of your post.

One rotation of 180 degrees around x followed by one of 90 around y. Would rotate from A to B

This is how I tried to solve it:

```<code># Quaternion to adjust reference system
first_quat = make_quaternion(unitary_x, pi) # Generates the quaternion that rotates pi around X
second_quat = make_quaternion(unitary_y, pi/2.0) # Generates the quaternion that rotates pi/2 around Y
composed_fs_q = first_quat*second_quat

# Quaternion to rotate from one reference system to the other
quaternion_ini_A = quaternion_ini_A*composed_fs_q
A2B_quaternion = quaternion_ini_B*(quaternion_ini_A.inverse())
</code>```

A2B_quaternion is the quaternion that I use for the rotation but still doesn't perform the right rotation. Any idea?

答:
In your particular case, isn't it easiest to just transpose the axes:

```<code>Bx =  Az
By = -Ay
Bz =  Ax
</code>```

Or if you want to write that as a matrix transform:

```<code>B = A * [ [  0,  0,  1  ],
          [  0, -1,  0  ],
          [  1,  0,  0  ] ]
</code>```

Body:
In your particular case, isn't it easiest to just transpose the axes:

Bx =  Az
By = -Ay
Bz =  Ax

Or if you want to write that as a matrix transform:

B = A * [ [  0,  0,  1  ],
          [  0, -1,  0  ],
          [  1,  0,  0  ] ]